更新排钻碰撞交集测试代码

src/Common/Matrix4Utils.ts

@@ -60,7 +60,7 @@ export function matrixScale(scale: number, center?: Vector3)
 }
 
 /**
- * 设置Mat的旋转矩阵
+ * 设置旋转矩阵,不改变矩阵的基点
  */
 export function setRotationOnAxis(mtx: Matrix4, axis: Vector3, ro: number)
 {
@@ -113,3 +113,20 @@ export function ApplyMatrix4IgnorePosition(vec: { x: number, y: number, z: numbe
     vec.z = e[2] * x + e[6] * y + e[10] * z;
     return vec;
 }
+
+/**
+ * 把变换矩阵展平成2d矩阵,避免出现三维坐标.
+ */
+export function MatrixPlanarizere(mtx: Matrix4, z0 = true)
+{
+    mtx.elements[2] = 0;
+    mtx.elements[6] = 0;
+    mtx.elements[8] = 0;
+    mtx.elements[9] = 0;
+    mtx.elements[10] = Math.sign(mtx.elements[10]);
+
+    if (z0)
+        mtx.elements[14] = 0;
+
+    return mtx;
+}

src/DatabaseServices/Entity/Entity.ts

@@ -2,7 +2,7 @@ import { Box3, Material, Matrix3, Matrix4, MeshStandardMaterial, Object3D, Vecto
 import { iaop } from 'xaop';
 import { HostApplicationServices } from '../../ApplicationServices/HostApplicationServices';
 import { DisposeThreeObj, Object3DRemoveAll } from '../../Common/Dispose';
-import { matrixIsCoplane } from '../../Common/Matrix4Utils';
+import { matrixIsCoplane, MatrixPlanarizere } from '../../Common/Matrix4Utils';
 import { UpdateDraw } from '../../Common/Status';
 import { ObjectSnapMode } from '../../Editor/ObjectSnapMode';
 import { userConfig } from '../../Editor/UserConfig';
@@ -110,12 +110,25 @@ export class Entity extends CADObject
         this.Update(UpdateDraw.Matrix);
     }
 
-    /** Z轴归0 */
+    //Z轴归0
     Z0()
     {
         this.WriteAllObjectRecord();
         this._Matrix.elements[14] = 0;
         this.Update(UpdateDraw.Matrix);
+        return this;
+    }
+
+    //坐标系二维化
+    MatrixPlanarizere()
+    {
+        let z = this._Matrix.elements[10];
+        if (equaln(Math.abs(z), 1, 1e-4))
+        {
+            this.WriteAllObjectRecord();
+            MatrixPlanarizere(this._Matrix, false);
+        }
+        return this;
     }
 
     get OCSInv(): Matrix4

src/DatabaseServices/Entity/Line.ts

@@ -155,7 +155,7 @@ export class Line extends Curve
         return this.EndPoint.sub(this.StartPoint);
     }
 
-    IntersectWith(curve: Curve, intType: IntersectOption, tolerance = 1e-6): Vector3[]
+    IntersectWith(curve: Curve, intType: IntersectOption, tolerance = 1e-4): Vector3[]
     {
         if (curve instanceof Line)
         {
@@ -189,7 +189,7 @@ export class Line extends Curve
     GetParamAtPoint(pt: Vector3): number
     {
         let { closestPt, param } = this.GetClosestAtPoint(pt, true);
-        if (!equalv3(closestPt, pt))
+        if (!equalv3(closestPt, pt, 1e-5))
             return NaN;
         return param;
     }

src/DatabaseServices/Entity/Polyline.ts

@@ -711,7 +711,7 @@ export class Polyline extends Curve
 
     MatrixAlignTo(toMatrix: Matrix4)
     {
-        if (!matrixIsCoplane(this._Matrix, toMatrix))
+        if (!matrixIsCoplane(this._Matrix, toMatrix, 1e-4))
             return;
 
         this.WriteAllObjectRecord();
@@ -1030,7 +1030,7 @@ export class Polyline extends Curve
         }
     }
 
-    IntersectWith(curve: Curve, intType: IntersectOption, tolerance = 1e-6): Vector3[]
+    IntersectWith(curve: Curve, intType: IntersectOption, tolerance = 1e-5): Vector3[]
     {
         return IntersectPolylineAndCurve(this, curve, intType, tolerance);
     }

src/DatabaseServices/Entity/Region.ts

@@ -139,6 +139,24 @@ export class Region extends Entity
         this.Update(UpdateDraw.Geometry);
         return this;
     }
+
+    //Z轴归0
+    Z0()
+    {
+        super.Z0();
+        for (let s of this.m_ShapeManager.ShapeList)
+            s.Z0();
+
+        return this;
+    }
+    MatrixPlanarizere()
+    {
+        super.MatrixPlanarizere();
+        for (let s of this.m_ShapeManager.ShapeList)
+            s.MatrixPlanarizere();
+        return this;
+    }
+
     protected ApplyMirrorMatrix(m: Matrix4)
     {
         return this;

src/DatabaseServices/Shape.ts

@@ -59,6 +59,13 @@ export class Shape
             h.Curve.Z0();
     }
 
+    MatrixPlanarizere()
+    {
+        this.m_Outline.Curve.MatrixPlanarizere();
+        for (let h of this.m_Holes)
+            h.Curve.MatrixPlanarizere();
+    }
+
     ApplyMatrix(m: Matrix4)
     {
         this.m_Outline.Curve.ApplyMatrix(m);

src/Geometry/DrillParse/BoardGetFace.ts

@@ -137,11 +137,7 @@ export class BoardGetFace
                 )
                     continue;
                 //不共面
-                if (!matrixIsCoplane(f1.OCS, f2.OCS, 1e-3))
-                    continue;
-                let nor1 = new Vector3().setFromMatrixColumn(f1.OCS, 2);
-                let nor2 = new Vector3().setFromMatrixColumn(f2.OCS, 2);
-                if (equalv3(nor1, nor2, 1e-6))
+                if (!MatrixIsCoplane2(f1.OCS, f2.OCS, 1e-3))
                     continue;
                 collisionFaces.push(...f1.Intersect(f2));
             }
@@ -150,3 +146,20 @@ export class BoardGetFace
     }
 }
 
+//坐标系共面且法线相反
+function MatrixIsCoplane2(matrixFrom: Matrix4, matrixTo: Matrix4, fuzz = 1e-5): boolean
+{
+    let nor1 = new Vector3().setFromMatrixColumn(matrixFrom, 2);
+    let nor2 = new Vector3().setFromMatrixColumn(matrixTo, 2);
+
+    //法线共面
+    if (!equalv3(nor1, nor2.negate(), fuzz))
+        return false;
+
+    //高共面
+    let pt = new Vector3().setFromMatrixPosition(matrixTo);
+    //变换到自身对象坐标系.
+    pt.applyMatrix4(new Matrix4().getInverse(matrixFrom));
+
+    return equaln(pt.z, 0, fuzz);
+}

src/Geometry/DrillParse/Face.ts

@@ -1,10 +1,11 @@
 import { Matrix4, Vector3 } from "three";
+import { MatrixPlanarizere } from "../../Common/Matrix4Utils";
 import { Board } from "../../DatabaseServices/Entity/Board";
 import { Line } from "../../DatabaseServices/Entity/Line";
 import { Region } from "../../DatabaseServices/Entity/Region";
 import { BoolOpeartionType } from "../../GraphicsSystem/BoolOperateUtils";
 import { Box3Ext } from "../Box";
-import { XAxis, equaln } from "../GeUtils";
+import { equaln, XAxis } from "../GeUtils";
 import { BoardFaceType } from "./BoardGetFace";
 
 export interface BoardFaceParams
@@ -80,17 +81,15 @@ export class Face
     Intersect(f: Face): Face[]
     {
         //获得侧面和非侧面
-        let [sideFace, noSideFace] = this.type === BoardFaceType.Side ?
-            [this, f] : [f, this];
+        let [sideFace, noSideFace] = this.type === BoardFaceType.Side ? [this, f] : [f, this];
 
-        let sideOcs = sideFace.OCS.clone();
-        let interMat = noSideFace.OCS.clone();
         //布尔面和被布尔面得差异矩阵
-        let diffMat = new Matrix4().multiplyMatrices(new Matrix4().getInverse(sideOcs), interMat);
+        let diffMtx = new Matrix4().getInverse(sideFace.OCS).multiply(noSideFace.OCS);
+        MatrixPlanarizere(diffMtx);
 
         let isSuccess = false;
 
-        let x = new Vector3().setFromMatrixColumn(diffMat, 0);
+        let x = new Vector3().setFromMatrixColumn(diffMtx, 0);
         let ang = x.angleTo(XAxis);
         //盒子旋转0,90,180度不会被破坏
         let canUseBoxCalc = equaln(ang, 0) || equaln(ang, Math.PI / 2) || equaln(ang, Math.PI);
@@ -102,7 +101,8 @@ export class Face
         if (!noSideFace.IsRect || !canUseBoxCalc)
         {
             let sideReg = sideFace.Region;
-            isSuccess = sideReg.BooleanOper(noSideFace.Region.Clone().ApplyMatrix(diffMat), BoolOpeartionType.Intersection);
+            let toReg = noSideFace.Region.Clone().ApplyMatrix(diffMtx);
+            isSuccess = sideReg.BooleanOper(toReg, BoolOpeartionType.Intersection);
             for (let s of sideReg.ShapeManager.ShapeList)
             {
                 let box = s.BoundingBox as Box3Ext;
@@ -114,8 +114,8 @@ export class Face
         {
             let retBox = new Box3Ext(new Vector3(), new Vector3(sideFace.m_Length, sideFace.m_Width));
 
-            let p1 = new Vector3().applyMatrix4(diffMat).setZ(0);
-            let p2 = new Vector3(noSideFace.m_Length, noSideFace.m_Width).applyMatrix4(diffMat).setZ(0);
+            let p1 = new Vector3().setFromMatrixPosition(diffMtx);
+            let p2 = new Vector3(noSideFace.m_Length, noSideFace.m_Width).applyMatrix4(diffMtx);
             let box3 = new Box3Ext().setFromPoints([p1, p2]);
 
             if (retBox.intersectsBox(box3))
@@ -143,10 +143,10 @@ export class Face
                 newFace.m_Width = sizes[i].y;
 
                 let min = retBoxs[i].min;
-                min.applyMatrix4(sideOcs);
+                min.applyMatrix4(sideFace.OCS);
 
                 //构建碰撞面坐标系
-                newFace.OCS = sideOcs.clone().setPosition(min);
+                newFace.OCS = sideFace.OCS.clone().setPosition(min);
 
                 newFace.IsPositiveFace = sideFace.IsPositiveFace;
                 newFace.drillType = sideFace.drillType;