experiment with spherical coordinates

modules/math/l3space.ts

@@ -353,6 +353,13 @@ export class Matrix3 {
   };
 
   apply = vector => this.__apply(vector, new Vector());
+
+  setTranslation(tx, ty, tz) {
+    this.tx = tx;
+    this.ty = ty;
+    this.tz = tz;
+    return this;
+  }
 }
 
 

modules/plugable/index.ts

@@ -0,0 +1,14 @@
+
+export interface Plugin<InContext, OutContext> {
+
+  id: string;
+
+  dependencies: string[];
+
+  activate(ctx: InContext & OutContext): () => void | void;
+
+}
+
+export function activatePlugins(plugins: Plugin<any, any>[], context: any) {
+
+}

web/app/cad/assembly/assembly.ts

@@ -3,6 +3,9 @@ import {Param} from "../../sketcher/shapes/param";
 import {ISolveStage, SolvableObject} from "../../sketcher/constr/solvableObject";
 import {AlgNumConstraint} from "../../sketcher/constr/ANConstraints";
 import {AssemblyCSysNode} from "./nodes/assemblyCSysNode";
+import {AssemblyOrientationNode} from "./nodes/assemblyOrientationNode";
+import {Constraints3D} from "./constraints3d";
+import {AssemblyLocationNode} from "./nodes/assemblyLocationNode";
 
 export abstract class AssemblyNode implements SolvableObject {
 
@@ -27,7 +30,11 @@ export abstract class AssemblyNode implements SolvableObject {
     return [];
   }
 
-  createRigidBodyLink(body: AssemblyCSysNode): AlgNumConstraint[] {
+  createOrientationRelationship(location: AssemblyLocationNode): AlgNumConstraint[] {
+    return [];
+  }
+
+  createTranslationRelationship(location: AssemblyLocationNode): AlgNumConstraint[] {
     return [];
   }
 

web/app/cad/assembly/assemblyPlugin.ts

@@ -2,45 +2,47 @@ import {ApplicationContext} from "context";
 import {ModellerContextualActions} from "./ui/ModellerContextualActions";
 import {state, StateStream} from "lstream";
 import {AssemblyConstraintDefinition} from "./assemblyConstraintDefinition";
-import {solveAssembly as solveAssemblyImpl} from "./assemblySolver";
+import {AssemblyProcess, launchAssembly} from "./assemblySolver";
-import {Constraints3D, createAssemblyConstraint} from "./constraints3d";
 import {SolveStatus} from "../../sketcher/constr/AlgNumSystem";
 import {ConstantsDefinitions} from "../../sketcher/constr/ANConstraints";
 import {AssemblyView} from "./ui/AssemblyView";
 import {IoMdConstruct} from "react-icons/io";
+import {AssemblyConstraints, Constraints3D} from "./constraints3d";
 
 export function activate(ctx: ApplicationContext) {
 
-  const constraints$ = state<AssemblyConstraintDefinition[][]>([]);
+  const constraints$ = state<AssemblyConstraintDefinition[]>([]);
   const status$ = state<SolveStatus>(null);
 
-  function getConstraints(): AssemblyConstraintDefinition[][] {
+  function getConstraints(): AssemblyConstraintDefinition[] {
     return constraints$.value;
   }
 
-  function loadConstraints(inData: AssemblyConstraintDefinition[][]): void {
+  function loadConstraints(inData: AssemblyConstraintDefinition[]): void {
+    inData = inData.filter(constr => {
+      const shouldBeFiltered = !AssemblyConstraints[constr.typeId];
+      if (shouldBeFiltered) {
+        console.log('Unknown constraint ' + constr.typeId + ' will be skipped');
+      }
+      return !shouldBeFiltered;
+    });
     constraints$.next(inData);
   }
 
   function addConstraint(typeId: string, objects: string[], constants?: ConstantsDefinitions): void {
-    constraints$.mutate(stages => {
+    constraints$.mutate(constraints => {
-      if (stages.length === 0) {
+      constraints.push({
-        stages.push([])
-      }
-      stages[stages.length - 1].push({
         typeId, objects, constants
       });
     })
   }
 
   function removeConstraint(constr: AssemblyConstraintDefinition) {
-    constraints$.mutate(stages => {
+    constraints$.mutate(constrs => {
-      for (let constrs of stages) {
       const index = constrs.indexOf(constr);
       if (index !== -1) {
         constrs.splice(index, 1);
       }
-      }
     })
   }
 
@@ -50,27 +52,13 @@ export function activate(ctx: ApplicationContext) {
       return;
     }
 
-    const stages = constraints$.value.map(stage => stage.map(constr => {
+    const constraints = constraints$.value;
-      const schema = Constraints3D[constr.typeId];
-      if (!schema) {
-        console.error('reference to nonexistent constraint ' + constr.typeId);
-        return null;
-      }
-      const objects = [];
-      for (const id of constr.objects) {
-        const modelObject = ctx.cadRegistry.find(id);
-        if (!modelObject) {
-          console.warn('skipping constraint referring to nonexistent object ' + id);
-          return null;
-        }
-        objects.push(modelObject);
-      }
-      return createAssemblyConstraint(schema, objects)
-    } ).filter(x => x) );
 
-    const solveStatus = solveAssemblyImpl(stages);
+    const assemblyProcess = new AssemblyProcess(ctx.cadRegistry, constraints);
 
-    status$.next(solveStatus);
+    launchAssembly(assemblyProcess);
+
+    status$.next(assemblyProcess.solveStatus);
   }
 
   constraints$.attach(solveAssembly);
@@ -93,7 +81,7 @@ export function activate(ctx: ApplicationContext) {
 
 export interface AssemblyService {
 
-  constraints$: StateStream<AssemblyConstraintDefinition[][]>;
+  constraints$: StateStream<AssemblyConstraintDefinition[]>;
 
   status$: StateStream<SolveStatus>;
 
@@ -103,9 +91,9 @@ export interface AssemblyService {
 
   solveAssembly(): void;
 
-  loadConstraints(constraints: AssemblyConstraintDefinition[][]);
+  loadConstraints(constraints: AssemblyConstraintDefinition[]);
 
-  getConstraints(): AssemblyConstraintDefinition[][];
+  getConstraints(): AssemblyConstraintDefinition[];
 
 }
 

web/app/cad/assembly/assemblySolver.ts

@@ -6,73 +6,242 @@ import {AssemblyNode} from "./assembly";
 import {ISolveStage} from "../../sketcher/constr/solvableObject";
 import {MShell} from "../model/mshell";
 import {AssemblyCSysNode} from "./nodes/assemblyCSysNode";
+import {AssemblyConstraints, AssemblyConstraintSchema, Constraints3D} from "./constraints3d";
+import {AssemblyConstraintDefinition} from "./assemblyConstraintDefinition";
+import {MObject} from "../model/mobject";
+import {CadRegistry} from "../craft/cadRegistryPlugin";
 
-export function solveAssembly(stages: AlgNumConstraint[][]): SolveStatus {
+export class RigidBody {
 
-  // temporary solve everything in one stage
+  model: MObject;
-  const constraints = [].concat(...stages);
+  relationships = new Map<RigidBody, AssemblyConstraint[]>();
 
-  const objects = new Set<AssemblyNode>();
+  reset() {
-  constraints.forEach(c => c.objects.forEach(o => objects.add(o)));
+    this.model.traverse(m => {
+      if (m.assemblyNodes) {
+        Object.values(m.assemblyNodes).forEach((node: AssemblyNode) => node.reset());
+      }
+    })
+  }
+}
 
-  const stage: ISolveStage = {
+export class AssemblyConstraint {
-    objects: objects,
+  objects: MObject[] = [];
-    index: 0
+  schema: AssemblyConstraintSchema;
+}
+
+export class AssemblyProcess {
+
+  queue: RigidBody[];
+  solved: Set<RigidBody> = new Set();
+  solveStatus: SolveStatus = {
+    success: true,
+    error: 0
   };
+  errorStep = null;
 
-  const roots = new Set<MShell>();
+  constructor(cadRegistry: CadRegistry, constraintDefs: AssemblyConstraintDefinition[]) {
-  objects.forEach(o => {
+    this.queue = buildAssemblyQueue(cadRegistry, constraintDefs)
-    const root = o.model.root;
+  }
-    if (root instanceof MShell) {
+
-      roots.add(root);
+  step() {
-      objects.add(root.assemblyNodes.location)
+
+    const body = this.queue.pop();
+    const constraints = [];
+    body.relationships.forEach((overConstraints, bodyBuddy) => {
+
+      if (this.solved.has(bodyBuddy)) {
+        overConstraints.forEach(c => constraints.push(c));
+      }
+    });
+    body.reset();
+
+    this.solveStatus = solve(constraints, body.model, true);
+    if (!this.solveStatus.success) {
+      this.errorStep = body.model.id;
+      console.log("Assembly system haven't been solved at the orientation step");
+      return;
+    }
+    // this.solveStatus = solve(constraints, body.model, false);
+    // if (!this.solveStatus.success) {
+    //   console.log("Assembly system haven't been solved at the translation step");
+    //   this.errorStep = body.model.id;
+    //   return;
+    // }
+
+    applyLocation(body.model as MShell);
+
+    this.solved.add(body);
+  }
+
+  isDone(): boolean {
+    return this.queue.length === 0;
+  }
+
+}
+
+
+function buildAssemblyQueue(cadRegistry: CadRegistry, constraintDefs: AssemblyConstraintDefinition[]): RigidBody[] {
+
+  const constraints: AssemblyConstraint[] = [];
+
+  constraintDefs.forEach(def => {
+    const schema = AssemblyConstraints[def.typeId];
+    if (!schema) {
+      console.error('reference to nonexistent constraint ' + def.typeId);
+      return null;
+    }
+    const constraint = new AssemblyConstraint();
+    constraint.schema = schema;
+    const objects = [];
+    for (const id of def.objects) {
+      const modelObject = cadRegistry.find(id);
+      if (!modelObject) {
+        console.warn('skipping constraint referring to nonexistent object ' + id);
+        return null;
+      }
+      constraint.objects.push(modelObject);
+      objects.push(modelObject);
+    }
+    constraints.push(constraint);
+  });
+
+
+  const bodies = new Map<MObject, RigidBody>();
+  function body(obj: MObject) {
+    let rigidBody = bodies.get(obj);
+    if (!rigidBody) {
+      rigidBody = new RigidBody();
+      rigidBody.model = obj;
+      bodies.set(obj, rigidBody);
+    }
+    return rigidBody;
+  }
+
+  function link(a: MObject, b: MObject, constr: AssemblyConstraint) {
+    const rigidBodyA = body(a);
+    const rigidBodyB = body(b);
+
+    let arr = rigidBodyA.relationships.get(rigidBodyB);
+    if (!arr) {
+      arr = [];
+      rigidBodyA.relationships.set(rigidBodyB, arr);
+    }
+    arr.push(constr)
+  }
+
+  constraints.forEach(constr => {
+    const roots = new Set<MObject>();
+    constr.objects.forEach(o => roots.add(o.root));
+    const arr: MObject[] = Array.from(roots);
+    for (let i = 0; i < arr.length; i++) {
+      for (let j = i+1; j < arr.length; j++) {
+        link(arr[i], arr[j], constr);
+        link(arr[j], arr[i], constr);
+      }
     }
   });
 
-  objects.forEach(o => {
+  return Array.from(bodies.values());
-    o.stage = stage;
+}
-    o.reset();
+
+export function launchAssembly(assemblyProcess: AssemblyProcess): void {
+
+  while (!assemblyProcess.isDone()) {
+    assemblyProcess.step();
+    if (assemblyProcess.errorStep !== null) {
+      break;
+    }
+  }
+
+}
+
+function addToStage(stage: ISolveStage, object: AssemblyNode) {
+  stage.objects.add(object);
+  object.stage = stage;
+}
+
+
+
+function solve(constraints: AssemblyConstraint[], freeBody: MObject, orientation: boolean): SolveStatus {
+
+
+  if (!(freeBody instanceof MShell)) {
+    throw 'unsupported: needs location implementation';
+  }
+
+  const readOnlyStage: ISolveStage = {
+    objects: new Set<AssemblyNode>(),
+    index: 0
+  };
+
+  const stage: ISolveStage = {
+    objects: new Set<AssemblyNode>(),
+    index: 1
+  };
+
+
+  // const assemblyNodes: AssemblyNode  = orientation ? : ;
+
+  const solvingConstraints = [];
+
+  constraints.forEach(c => {
+    const nodes = c.schema.defineAssemblyScope(c.objects);
+
+    nodes.forEach(o => {
+      if (o.model.root === freeBody) {
+        addToStage(stage, o);
+      } else {
+        addToStage(readOnlyStage, o);
+      }
     });
 
+    solvingConstraints.push(new AlgNumConstraint(orientation ? c.schema.orientation : c.schema.translation, nodes));
+  });
+
+  addToStage(stage, freeBody.assemblyNodes.location);
 
   const system = new AlgNumSubSystem(() => 0.001, val => val, stage);
-
   system.startTransaction();
-  constraints.forEach(c => system.addConstraint(c));
+  solvingConstraints.forEach(c => {
-  objects.forEach(assemblyNode => {
+    system.addConstraint(c);
+  });
+  stage.objects.forEach(solveObject => {
+    const assemblyNode = solveObject as AssemblyNode;
     const internalConstraints = assemblyNode.createConsistencyConstraints();
     internalConstraints.forEach(c => {
       c.internal = true;
       system.addConstraint(c);
     });
+    if (assemblyNode.model.root === freeBody) {
+
+      const rigidBodyLinks = orientation ?
+        assemblyNode.createOrientationRelationship(freeBody.assemblyNodes.location):
+        assemblyNode.createTranslationRelationship(freeBody.assemblyNodes.location);
 
-    const root = assemblyNode.model.root;
-    if (root instanceof MShell) {
-      const rigidBodyLinks = assemblyNode.createRigidBodyLink(root.assemblyNodes.location);
       rigidBodyLinks.forEach(c => {
         c.internal = true;
         system.addConstraint(c);
       });
-    } else {
-      throw 'unsupported: needs location implementation';
     }
   });
 
   system.finishTransaction();
   system.solveFine();
 
-  if (system.solveStatus.success) {
-    roots.forEach(root => {
-      applyResults(root, root.assemblyNodes.location);
-    });
-  } else {
-    console.log("Assembly system haven't been solved, locations won't be updated");
-  }
-
   return system.solveStatus;
 }
 
 
+function applyLocation(shell: MShell): void {
+
+  const targetLocation = shell.assemblyNodes.location;
+
+
+
+  shell.location$.update(mx => targetLocation.toMatrix());
+}
+
 function applyResults(shell: MShell, targetCsysParams: AssemblyCSysNode): void {
   const [
     ox, oy, oz, ix, iy, iz, jx, jy, jz, kx, ky, kz
@@ -97,9 +266,9 @@ function applyResults(shell: MShell, targetCsysParams: AssemblyCSysNode): void {
   const tr = shell.csys.inTransformation3x3;
   basis.forEach(r => tr._apply(r));
 
-  shell.location$.update(csys => {
+  // shell.location$.update(csys => {
-    return targetCsys;
+  //   return targetCsys;
-  });
+  // });
   // shell.location$.mutate(csys => {
   //   csys.x = basis[0];
   //   csys.y = basis[1];
@@ -108,3 +277,4 @@ function applyResults(shell: MShell, targetCsysParams: AssemblyCSysNode): void {
   // });
 
 }
+

web/app/cad/assembly/constraints3d.ts

@@ -1,109 +1,98 @@
-import {Polynomial, POW_1_FN, POW_2_FN} from "../../sketcher/constr/polynomial";
+import {COS_FN, Polynomial, POW_1_FN, POW_2_FN, SIN_FN} from "../../sketcher/constr/polynomial";
 import {NoIcon} from "../../sketcher/icons/NoIcon";
-import {AlgNumConstraint, ConstantsDefinitions, ConstraintSchema} from "../../sketcher/constr/ANConstraints";
+import {ConstraintSchema} from "../../sketcher/constr/ANConstraints";
 import {MObject} from "../model/mobject";
-import {SolvableObject} from "../../sketcher/constr/solvableObject";
 import {AssemblyNode} from "./assembly";
-import {EndPoint} from "../../sketcher/shapes/point";
+import {IconType} from "react-icons";
-import {Circle} from "../../sketcher/shapes/circle";
+import Vector from "math/vector";
-import {Arc} from "../../sketcher/shapes/arc";
 
 
 export const Constraints3D = {
 
-  FaceParallel: {
+  PlaneOppositeNormals: {
-    id: 'FaceParallel',
+    id: 'PlaneOppositeNormals',
-    name: 'Face Parallel',
+    name: 'Plane Opposite Normals',
     icon: NoIcon,
 
-    defineAssemblyScope: ([face1, face2]) => {
-      return [
-        face1.assemblyNodes.normal,
-        face2.assemblyNodes.normal
-      ];
-    },
-
-    defineParamsScope: ([n1, n2], cb) => {
-      n1.visitParams(cb);
-      n2.visitParams(cb);
-    },
-
-    collectPolynomials: (polynomials, params) => {
-
-      const [
-        nx1, ny1, nz1, nx2, ny2, nz2,
-      ] = params;
-
-      polynomials.push(
-        new Polynomial(1)
-          .monomial()
-            .term(nx1, POW_1_FN)
-            .term(nx2, POW_1_FN)
-          .monomial()
-            .term(ny1, POW_1_FN)
-            .term(ny2, POW_1_FN)
-          .monomial()
-            .term(nz1, POW_1_FN)
-            .term(nz2, POW_1_FN)
-
-      );
-
-    }
-
-  },
-
-  FaceToFace: {
-    id: 'FaceToFace',
-    name: 'Face To Face',
-    icon: NoIcon,
-
-    selectionMatcher: {
-      selector: 'matchAll',
-      types: ['face'],
-      minQuantity: 2
-    },
-
-    defineAssemblyScope: ([face1, face2]) => {
-      return [
-        face1.assemblyNodes.plane,
-        face2.assemblyNodes.plane,
-      ];
-    },
-
     defineParamsScope: ([plane1, plane2], cb) => {
-      plane1.visitParams(cb);
+      cb(plane1.theta);
-      plane2.visitParams(cb);
+      cb(plane1.phi);
+      cb(plane2.theta);
+      cb(plane2.phi);
     },
 
     collectPolynomials: (polynomials, params) => {
 
       const [
-        nx1, ny1, nz1, w1, nx2, ny2, nz2, w2
+        theta1, phi1, theta2, phi2
+      ] = params;
+
+      // nx1, ny1, nz1, nx2, ny2, nz2
+
+      // sin(theta) * cos(phi),
+      // sin(theta) * sin(phi),
+      // cos(theta),
+
+
+      // const p = new Polynomial(1)
+      //   .monomial()
+      //     .term(theta1, SIN_FN)
+      //     .term(phi1, COS_FN)
+      //     .term(theta2, SIN_FN)
+      //     .term(phi2, COS_FN)
+      //   .monomial()
+      //     .term(theta1, SIN_FN)
+      //     .term(phi1, SIN_FN)
+      //
+      //     .term(theta2, SIN_FN)
+      //     .term(phi2, SIN_FN)
+      //   .monomial()
+      //     .term(theta1, COS_FN)
+      //     .term(theta2, COS_FN);
+
+      // 180 - theta1
+      polynomials.push(
+        new Polynomial(Math.PI)
+          .monomial(-1)
+            .term(theta1, POW_1_FN)
+          .monomial(-1)
+            .term(theta2, POW_1_FN)
+      );
+      polynomials.push(
+        new Polynomial(Math.PI)
+          .monomial(1)
+            .term(phi1, POW_1_FN)
+          .monomial(-1)
+            .term(phi2, POW_1_FN)
+      );
+    }
+  },
+
+  PlaneEqualDepth: {
+    id: 'PlaneEqualDepth',
+    name: 'Plane Equal Depth',
+    icon: NoIcon,
+
+    defineParamsScope: ([plane1, plane2], cb) => {
+      cb(plane1.w);
+      cb(plane2.w);
+    },
+
+    collectPolynomials: (polynomials, params) => {
+
+      const [
+        w1, w2
       ] = params;
 
       polynomials.push(
-        new Polynomial(1)
+        new Polynomial(0)
-          .monomial()
+          .monomial(1)
-            .term(nx1, POW_1_FN)
-            .term(nx2, POW_1_FN)
-          .monomial()
-            .term(ny1, POW_1_FN)
-            .term(ny2, POW_1_FN)
-          .monomial()
-            .term(nz1, POW_1_FN)
-            .term(nz2, POW_1_FN)
-
-      );
-      polynomials.push(
-        new Polynomial()
-          .monomial()
             .term(w1, POW_1_FN)
           .monomial()
             .term(w2, POW_1_FN)
+
       );
-
     }
-
   },
 
   UnitVectorConsistency: {
@@ -224,75 +213,181 @@ export const Constraints3D = {
     },
   },
 
-  RigidBodyPlaneLink: {
+  PlaneNormalLink: {
-    id: 'RigidBodyPlaneLink',
+    id: 'PlaneNormalLink',
-    name: 'RigidBodyPlaneLink',
+    name: 'Plane Normal Link',
     icon: NoIcon,
 
-    defineParamsScope: ([csys, plane], cb) => {
+    defineParamsScope: ([location, plane], cb) => {
-      csys.visitParams(cb);
+      cb(location.alpha);
-      plane.visitParams(cb);
+      cb(location.beta);
+      cb(location.gamma);
+
+      cb(plane.theta);
+      cb(plane.phi);
+
+
     },
 
     collectPolynomials: (polynomials, params, _, objects) => {
       const [csys, plane] = objects;
 
-      const n = plane.getNormal();
+      const {x: nStarX, y: nStarY, z: nStarZ} = plane.getNormal();
-      const wStar = plane.getDepth();
 
-      const {x: xStar, y: yStar, z: zStar} = n.multiply(wStar);
+      const [alpha, beta, gamma, theta, phi] = params;
 
-      const [ox, oy, oz, ix, iy, iz, jx, jy, jz, kx, ky, kz, x, y, z, w] = params;
+      // return new Vector(
+      //   Math.sin(theta) * Math.cos(phi),
+      //   Math.sin(theta) * Math.sin(phi),
+      //   Math.cos(theta),
+      // )
+
+      // out.x = this.mxx * x + this.mxy * y + this.mxz * z + this.tx;
+      // out.y = this.myx * x + this.myy * y + this.myz * z + this.ty;
+      // out.z = this.mzx * x + this.mzy * y + this.mzz * z + this.tz;
+
+      // cos(alpha)*cos(beta), cos(alpha)*sin(beta)*sin(gamma) - sin(alpha)*cos(gamma), cos(alpha)*sin(beta)*cos(gamma) + sin(alpha)*sin(gamma),
+      // sin(alpha)*cos(beta), sin(alpha)*sin(beta)*sin(gamma) + cos(alpha)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - cos(alpha)*sin(gamma),
+      // -sin(beta), cos(beta)*sin(gamma), cos(beta)*cos(gamma)
+
+      polynomials.push(
+        new Polynomial(0)
+          .monomial(-1)
+            .term(theta, SIN_FN)
+            .term(phi, COS_FN)
+          .monomial(nStarX)
+            .term(alpha, COS_FN)
+            .term(beta, COS_FN)
+
+          .monomial(nStarY)
+            .term(alpha, COS_FN)
+            .term(beta,  SIN_FN)
+            .term(gamma, SIN_FN)
+
+          .monomial(-nStarY)
+            .term(alpha, SIN_FN)
+            .term(gamma, COS_FN)
+
+          .monomial(nStarZ)
+            .term(alpha, COS_FN)
+            .term(beta,  SIN_FN)
+            .term(gamma, COS_FN)
+
+          .monomial(nStarZ)
+            .term(alpha,  SIN_FN)
+            .term(gamma,  SIN_FN)
+
+      );
+
+      // sin(alpha)*cos(beta), sin(alpha)*sin(beta)*sin(gamma) + cos(alpha)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - cos(alpha)*sin(gamma),
+      //   Math.sin(theta) * Math.sin(phi),
+
+      polynomials.push(
+        new Polynomial(0)
+          .monomial(-1)
+            .term(theta, SIN_FN)
+            .term(phi, SIN_FN)
+
+          .monomial(nStarX)
+            .term(alpha, SIN_FN)
+            .term(beta, COS_FN)
+
+          .monomial(nStarY)
+            .term(alpha, SIN_FN)
+            .term(beta,  SIN_FN)
+            .term(gamma, SIN_FN)
+
+          .monomial(nStarY)
+            .term(alpha, COS_FN)
+            .term(gamma, COS_FN)
+
+          .monomial(nStarZ)
+            .term(alpha, SIN_FN)
+            .term(beta,  SIN_FN)
+            .term(gamma, COS_FN)
+
+          .monomial(-nStarZ)
+            .term(alpha,  COS_FN)
+            .term(gamma,  SIN_FN)
+
+      );
+
+      // -sin(beta), cos(beta)*sin(gamma), cos(beta)*cos(gamma)
+
+      polynomials.push(
+        new Polynomial(0)
+          .monomial(-1)
+            .term(theta, COS_FN)
+
+          .monomial(-nStarX)
+            .term(beta, SIN_FN)
+
+          .monomial(nStarY)
+            .term(beta,  COS_FN)
+            .term(gamma, SIN_FN)
+
+          .monomial(nStarZ)
+            .term(beta,  COS_FN)
+            .term(gamma, COS_FN)
+      );
+
+    }
+
+  },
+
+  PlaneDepthLink: {
+    id: 'PlaneDepthLink',
+    name: 'PlaneDepthLink',
+    icon: NoIcon,
+
+    defineParamsScope: ([location, plane], cb) => {
+      cb(location.dx);
+      cb(location.dy);
+      cb(location.dz);
+      cb(plane.w);
+    },
+
+    collectPolynomials: (polynomials, params, _, objects) => {
+      const [location, plane] = objects;
+      const [ox, oy, oz, w] = params;
+      const {x: xP, y: yP, z: zP} = plane.toNormalVector();
+
+      // __DEBUG__.AddNormal()
+
+      const {x: nStarX, y: nStarY, z: nStarZ} = plane.getNormal();
+      const nStarW = plane.getDepth();
+
+      const pStar = plane.getNormal().multiply(nStarW);
+      const p0 = location.rotationMatrix().apply(pStar);
+      const w0 = p0.length();
 
       // out.x = this.mxx * x + this.mxy * y + this.mxz * z + this.tx;
       // out.y = this.myx * x + this.myy * y + this.myz * z + this.ty;
       // out.z = this.mzx * x + this.mzy * y + this.mzz * z + this.tz;
 
       polynomials.push(
-        new Polynomial(0)
+        new Polynomial(-xP * w0)
-          .monomial(-1)
+            .monomial(xP)
-            .term(x, POW_1_FN)
           .term(w, POW_1_FN)
-          .monomial(xStar)
+            .monomial(-1)
-            .term(ix, POW_1_FN)
-          .monomial(yStar)
-            .term(jx, POW_1_FN)
-          .monomial(zStar)
-            .term(kx, POW_1_FN)
-          .monomial()
           .term(ox, POW_1_FN)
       );
 
       polynomials.push(
-        new Polynomial(0)
+        new Polynomial(-yP * w0)
-          .monomial(-1)
+          .monomial(yP)
-            .term(y, POW_1_FN)
             .term(w, POW_1_FN)
-          .monomial(xStar)
+          .monomial(-1)
-            .term(iy, POW_1_FN)
-          .monomial(yStar)
-            .term(jy, POW_1_FN)
-          .monomial(zStar)
-            .term(ky, POW_1_FN)
-          .monomial()
             .term(oy, POW_1_FN)
 
       );
 
       polynomials.push(
-        new Polynomial(0)
+        new Polynomial(-zP * w0)
-          .monomial(-1)
+          .monomial(zP)
-            .term(z, POW_1_FN)
             .term(w, POW_1_FN)
-          .monomial(xStar)
+          .monomial(-1)
-            .term(iz, POW_1_FN)
-          .monomial(yStar)
-            .term(jz, POW_1_FN)
-          .monomial(zStar)
-            .term(kz, POW_1_FN)
-          .monomial()
             .term(oz, POW_1_FN)
-
       );
 
     }
@@ -313,7 +408,9 @@ export const Constraints3D = {
       cb(csys.kx);
       cb(csys.ky);
       cb(csys.kz);
-      vec.visitParams(cb);
+      cb(vec.x);
+      cb(vec.y);
+      cb(vec.z);
     },
 
     collectPolynomials: (polynomials, params, _, objects) => {
@@ -450,20 +547,69 @@ export const Constraints3D = {
 };
 
 
-export interface AssemblyConstraintSchema extends ConstraintSchema {
+export const AssemblyConstraints: {
+  [key: string]: AssemblyConstraintSchema
+} = {
+
+  FaceToFace: {
+    id: 'FaceToFace',
+    name: 'Face To Face',
+    icon: NoIcon,
+
+    selectionMatcher: {
+      selector: 'matchAll',
+      types: ['face'],
+      minQuantity: 2
+    },
+
+    defineAssemblyScope: ([face1, face2]) => {
+
+      return [
+        face1.assemblyNodes.plane,
+        face2.assemblyNodes.plane,
+      ];
+    },
+
+    orientation: ([plane1, plane2]) => {
+
+      return new OrientationConstraint(plane1.);
+
+    },
+
+    translation: Constraints3D.PlaneEqualDepth,
+
+  }
+
+};
+
+export class OrientationConstraint {
+
+  vecA: Vector;
+  vecB: Vector;
+
+  constructor(vecA: Vector, vecB: Vector) {
+    this.vecA = vecA;
+    this.vecB = vecB;
+  }
+
+}
+
+export interface AssemblyConstraintSchema {
+
+  id: string,
+  name: string,
+  icon?: IconType,
+
   selectionMatcher?: {
     selector: string,
     types: any[],
     minQuantity: number
   };
-  defineAssemblyScope: (objects: MObject[]) => AssemblyNode[],
+
+  defineAssemblyScope: (objects: MObject[]) => AssemblyNode[];
+
+  orientation: (objects: AssemblyNode[]) => OrientationConstraint,
+  translation: ConstraintSchema,
+
 }
 
-
-export function createAssemblyConstraint(schema: AssemblyConstraintSchema,
-                                         objects: MObject[],
-                                         constants?: ConstantsDefinitions,
-                                         internal: boolean = false) {
-
-  return new AlgNumConstraint(schema, schema.defineAssemblyScope(objects), constants, internal);
-}

web/app/cad/assembly/nodes/assemblyCSysNode.ts

@@ -4,6 +4,7 @@ import {MObject} from "../../model/mobject";
 import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
 import {Constraints3D} from "../constraints3d";
 import {AssemblyNode} from "../assembly";
+import Vector from "math/vector";
 
 export class AssemblyCSysNode extends AssemblyNode {
 
@@ -61,6 +62,20 @@ export class AssemblyCSysNode extends AssemblyNode {
 
   }
 
+
+  rotationMatrix() {
+    const {
+      ix, iy, iz, jx, jy, jz, kx, ky, kz
+      } = this;
+
+    return new Matrix3().setBasis([
+      new Vector(ix.get(), iy.get(), iz.get()),
+      new Vector(jx.get(), jy.get(), jz.get()),
+      new Vector(kx.get(), ky.get(), kz.get()),
+    ]);
+
+  }
+
   createConsistencyConstraints() {
     return [
       new AlgNumConstraint(Constraints3D.CSysConsistency, [this])

web/app/cad/assembly/nodes/assemblyLocationNode.ts

@@ -0,0 +1,81 @@
+import {Param} from "../../../sketcher/shapes/param";
+import {Matrix3} from "math/l3space";
+import {MObject} from "../../model/mobject";
+import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
+import {Constraints3D} from "../constraints3d";
+import {AssemblyNode} from "../assembly";
+import Vector from "math/vector";
+
+export class AssemblyLocationNode extends AssemblyNode {
+
+  alpha = new Param(0, 'A');
+  beta  = new Param(0, 'B');
+  gamma  = new Param(0, 'G');
+  dx = new Param(0, 'X');
+  dy  = new Param(0, 'Y');
+  dz  = new Param(0, 'Z');
+
+  getTransformation: () => Matrix3;
+
+  constructor(model: MObject, getTransformation: () => Matrix3) {
+    super(model);
+    this.getTransformation = getTransformation;
+  }
+
+  visitParams(cb) {
+    cb(this.alpha);
+    cb(this.beta);
+    cb(this.gamma);
+  }
+
+  reset() {
+    const mx = this.getTransformation();
+    this.alpha.set(0);
+    this.beta.set(0);
+    this.gamma.set(0);
+  }
+
+
+  rotationMatrix() {
+
+    return new Matrix3().set3(
+
+      ...this.rotationComponents()
+
+    );
+
+  }
+
+  rotationComponents(): [number, number, number, number, number, number, number, number, number] {
+
+    const alpha = this.alpha.get();
+    const beta = this.beta.get();
+    const gamma = this.gamma.get();
+
+    const cos = Math.cos;
+    const sin = Math.sin;
+
+    return [
+      cos(alpha)*cos(beta), cos(alpha)*sin(beta)*sin(gamma) - sin(alpha)*cos(gamma), cos(alpha)*sin(beta)*cos(gamma) + sin(alpha)*sin(gamma),
+      sin(alpha)*cos(beta), sin(alpha)*sin(beta)*sin(gamma) + cos(alpha)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - cos(alpha)*sin(gamma),
+      -sin(beta), cos(beta)*sin(gamma), cos(beta)*cos(gamma)
+    ]
+  }
+
+  translationComponents(): [number, number, number] {
+    return [this.dx.get(), this.dy.get(), this.dz.get()];
+  }
+
+  toMatrix() {
+    const mx = this.rotationMatrix();
+    mx.setTranslation(...this.translationComponents());
+    return mx;
+  }
+
+  createConsistencyConstraints() {
+    return [
+      // new AlgNumConstraint(Constraints3D.CSysConsistency, [this])
+    ];
+  }
+
+}

web/app/cad/assembly/nodes/assemblyOrientationNode.ts

@@ -0,0 +1,61 @@
+import {Param} from "../../../sketcher/shapes/param";
+import {Matrix3} from "math/l3space";
+import {MObject} from "../../model/mobject";
+import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
+import {Constraints3D} from "../constraints3d";
+import {AssemblyNode} from "../assembly";
+
+export class AssemblyOrientationNode extends AssemblyNode {
+
+  ix = new Param(1, 'X');
+  iy = new Param(0, 'Y');
+  iz = new Param(0, 'Z');
+  jx = new Param(0, 'X');
+  jy = new Param(1, 'Y');
+  jz = new Param(0, 'Z');
+  kx = new Param(0, 'X');
+  ky = new Param(0, 'Y');
+  kz = new Param(1, 'Z');
+  getTransformation: () => Matrix3;
+
+  constructor(model: MObject, getTransformation: () => Matrix3) {
+    super(model);
+    this.getTransformation = getTransformation;
+  }
+
+  visitParams(cb) {
+    cb(this.ix);
+    cb(this.iy);
+    cb(this.iz);
+    cb(this.jx);
+    cb(this.jy);
+    cb(this.jz);
+    cb(this.kx);
+    cb(this.ky);
+    cb(this.kz);
+  }
+
+  reset() {
+    const mx = this.getTransformation();
+
+    this.ix.set(mx.mxx);
+    this.iy.set(mx.myx);
+    this.iz.set(mx.mzx);
+
+    this.jx.set(mx.mxy);
+    this.jy.set(mx.myy);
+    this.jz.set(mx.mzy);
+
+    this.kx.set(mx.mxz);
+    this.ky.set(mx.myz);
+    this.kz.set(mx.mzz);
+
+  }
+
+  createConsistencyConstraints() {
+    return [
+      new AlgNumConstraint(Constraints3D.CSysConsistency, [this])
+    ];
+  }
+
+}

web/app/cad/assembly/nodes/assemblyPlaneNode.ts

@@ -5,12 +5,13 @@ import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
 import {Constraints3D} from "../constraints3d";
 import {AssemblyNode} from "../assembly";
 import {AssemblyCSysNode} from "./assemblyCSysNode";
+import {clamp} from "../../../math/math";
+import {AssemblyLocationNode} from "./assemblyLocationNode";
 
 export class AssemblyPlaneNode extends AssemblyNode {
 
-  x = new Param(0, 'X');
+  theta = new Param(0, 'T');
-  y = new Param(0, 'Y');
+  phi  = new Param(0, 'P');
-  z = new Param(0, 'Z');
   w = new Param(0, 'W');
   getNormal: () => Vector;
   getDepth: () => number;
@@ -22,32 +23,46 @@ export class AssemblyPlaneNode extends AssemblyNode {
   }
 
   visitParams(cb) {
-    cb(this.x);
+    cb(this.theta);
-    cb(this.y);
+    cb(this.phi);
-    cb(this.z);
     cb(this.w);
   }
 
   reset() {
     const {x, y, z} = this.getNormal();
     const w = this.getDepth();
-    this.x.set(x);
+    const phi = Math.atan2(y, x);
-    this.y.set(y);
+    const theta = Math.acos(clamp(z, -1, 1));
-    this.z.set(z);
+
+    this.theta.set(theta);
+    this.phi.set(phi);
+
     this.w.set(w);
   }
 
+  toNormalVector() {
+    const theta = this.theta.get();
+    const phi = this.phi.get();
+    return new Vector(
+      Math.sin(theta) * Math.cos(phi),
+      Math.sin(theta) * Math.sin(phi),
+      Math.cos(theta),
+    )
+  }
+
   createConsistencyConstraints() {
     return [
-      new AlgNumConstraint(Constraints3D.UnitVectorConsistency, [this])
+
     ];
   }
 
 
-  createRigidBodyLink(body: AssemblyCSysNode) {
+  createOrientationRelationship(location: AssemblyLocationNode): AlgNumConstraint[] {
-    return [
+    return [new AlgNumConstraint(Constraints3D.PlaneNormalLink, [location, this])];
-      new AlgNumConstraint(Constraints3D.RigidBodyPlaneLink, [body, this])
+  }
-    ];
+
+  createTranslationRelationship(location: AssemblyLocationNode): AlgNumConstraint[] {
+    return [new AlgNumConstraint(Constraints3D.PlaneDepthLink, [location, this])];
   }
 
 }

web/app/cad/assembly/nodes/assemblyScalarNode.ts

@@ -1,9 +1,6 @@
 import {AssemblyNode} from "../assembly";
 import {Param} from "../../../sketcher/shapes/param";
 import {MObject} from "../../model/mobject";
-import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
-import {Constraints3D} from "../constraints3d";
-import {AssemblyCSysNode} from "./assemblyCSysNode";
 
 export class AssemblyScalarNode extends AssemblyNode {
 
@@ -24,10 +21,4 @@ export class AssemblyScalarNode extends AssemblyNode {
     cb(this.param);
   }
 
-  createRigidBodyLink(body: AssemblyCSysNode) {
-    return [
-      // new AlgNumConstraint(Constraints3D.RigidTest, [body, this.model.assemblyNodes.normal, this])
-    ];
-  }
-
 }

web/app/cad/assembly/nodes/assemblyTranslationNode.ts

@@ -0,0 +1,40 @@
+import {Param} from "../../../sketcher/shapes/param";
+import Vector from "math/vector";
+import {MObject} from "../../model/mobject";
+import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
+import {Constraints3D} from "../constraints3d";
+import {AssemblyNode} from "../assembly";
+import {AssemblyCSysNode} from "./assemblyCSysNode";
+
+export class AssemblyTranslationNode extends AssemblyNode {
+
+  x = new Param(0, 'X');
+  y = new Param(0, 'Y');
+  z = new Param(0, 'Z');
+  getVector: () => Vector;
+
+  constructor(model: MObject, getVector: () => Vector) {
+    super(model);
+    this.getVector = getVector;
+  }
+
+  visitParams(cb) {
+    cb(this.x);
+    cb(this.y);
+    cb(this.z);
+  }
+
+  reset() {
+    const {x, y, z} = this.getVector();
+    this.x.set(x);
+    this.y.set(y);
+    this.z.set(z);
+  }
+
+  createRigidBodyLink(body: AssemblyCSysNode) {
+    return [
+
+    ];
+  }
+
+}

web/app/cad/assembly/nodes/assemblyUnitVectorNode.ts

@@ -4,7 +4,7 @@ import {MObject} from "../../model/mobject";
 import {AlgNumConstraint} from "../../../sketcher/constr/ANConstraints";
 import {Constraints3D} from "../constraints3d";
 import {AssemblyNode} from "../assembly";
-import {AssemblyCSysNode} from "./assemblyCSysNode";
+import {AssemblyOrientationNode} from "./assemblyOrientationNode";
 
 export class AssemblyUnitVectorNode extends AssemblyNode {
 
@@ -37,10 +37,9 @@ export class AssemblyUnitVectorNode extends AssemblyNode {
     ];
   }
 
-  createRigidBodyLink(body: AssemblyCSysNode) {
+  createRigidBodyOrientationRelationship(orientationNode: AssemblyOrientationNode): AlgNumConstraint[] {
-    return [
+    return [new AlgNumConstraint(Constraints3D.RigidBodyLink3x3, [orientationNode, this])];
-      // new AlgNumConstraint(Constraints3D.RigidBodyLink3x3, [body, this])
-    ];
   }
 
+
 }

web/app/cad/assembly/ui/AssemblyView.tsx

@@ -1,8 +1,8 @@
-import React, {useContext, useEffect} from 'react';
+import React, {useContext, useEffect, useState} from 'react';
 import {useStream} from "ui/effects";
 import {Status} from "ui/components/Status";
 import Folder from "ui/components/Folder";
-import {Constraints3D} from "../constraints3d";
+import {AssemblyConstraints, Constraints3D} from "../constraints3d";
 import {AppContext} from "../../dom/components/AppContext";
 import cx from 'classnames';
 import {NoIcon} from "../../../sketcher/icons/NoIcon";
@@ -10,6 +10,8 @@ import ls from "../../../sketcher/components/ConstraintExplorer.less";
 import Fa from "ui/components/Fa";
 import {AssemblyConstraintDefinition} from "../assemblyConstraintDefinition";
 import {ApplicationContext} from "context";
+import {AssemblyProcess} from "../assemblySolver";
+import {StepByStepSimulation} from "./StepByStepSimulation";
 
 
 export function AssemblyView() {
@@ -23,9 +25,8 @@ export function AssemblyView() {
     <div>
       Status: <Status success={status.success} />
     </div>
-    {constraints.map((stage, i) => <Folder key={i} title={'Stage' + (i + 1)}>
+    {constraints.map((constr, i) => <AssemblyConstraintButton key={i} prefix={(i+1) + '.'} constraint={constr} />)}
-      {stage.map((constr, j) => <AssemblyConstraintButton key={j} prefix={j + '.'} constraint={constr} />) }
+    <StepByStepSimulation />
-    </Folder>)}
   </div>
 
 }
@@ -64,7 +65,7 @@ export function AssemblyConstraintButton({prefix='', constraint: c, ...props}: {
 
   useEffect(() => withdraw, [c]);
 
-  const schema = Constraints3D[c.typeId];
+  const schema = AssemblyConstraints[c.typeId];
   if (schema === null) {
     return <div className='warning-text'>Invalid Constraint {c.typeId} </div>
   }

web/app/cad/assembly/ui/ModellerContextualActions.tsx

@@ -2,7 +2,7 @@ import React, {useContext} from 'react';
 import {AppContext} from "../../dom/components/AppContext";
 import {useStream} from "ui/effects";
 import {Dialog} from "ui/components/Dialog";
-import {AssemblyConstraintSchema, Constraints3D} from "../constraints3d";
+import {AssemblyConstraints, AssemblyConstraintSchema, Constraints3D} from "../constraints3d";
 import {matchAvailableSubjects, MatchIndex, matchSelection} from "../../../sketcher/selectionMatcher";
 
 export function ModellerContextualActions() {
@@ -17,7 +17,7 @@ export function ModellerContextualActions() {
 
   const entities = selection.map(ctx.cadRegistry.find);
 
-  const allConstraints = Object.values(Constraints3D) as AssemblyConstraintSchema[];
+  const allConstraints = Object.values(AssemblyConstraints) as AssemblyConstraintSchema[];
   const availableConstraints = matchAvailableSubjects(entities, allConstraints) as AssemblyConstraintSchema[];
 
   if (availableConstraints.length === 0) {

web/app/cad/assembly/ui/StepByStepSimulation.tsx

@@ -0,0 +1,34 @@
+import React, {useContext, useState} from "react";
+import {AppContext} from "../../dom/components/AppContext";
+import {AssemblyProcess} from "../assemblySolver";
+import {useStream} from "ui/effects";
+import {MShell} from "../../model/mshell";
+import CSys from "math/csys";
+import {Matrix3} from "math/l3space";
+
+export function StepByStepSimulation() {
+
+  const ctx = useContext(AppContext);
+
+  const [process, setProcess] = useState<AssemblyProcess>(null);
+  const constraints = useStream(ctx => ctx.assemblyService.constraints$);
+
+
+  function stepByStepSimulation() {
+    if (process === null || process.isDone()) {
+      const newProcess = new AssemblyProcess(ctx.cadRegistry, constraints);
+      newProcess.queue.forEach(rb => {
+        if (rb.model.root instanceof MShell) {
+          rb.model.root.location$.next(new Matrix3());
+        }
+      });
+      newProcess.step();
+      setProcess(newProcess);
+    } else {
+      process.step();
+    }
+  }
+
+  return <button onClick={stepByStepSimulation}>step</button>
+
+}

web/app/cad/debugPlugin.js

@@ -216,11 +216,11 @@ function addGlobalDebugActions({viewer, cadScene, cadRegistry}) {
       viewer.render();
     },
     HideSolids: () => {
-      cadRegistry.getAllShells().forEach(s => s.cadGroup.traverse(o => o.visible = false));
+      cadRegistry.getAllShells().forEach(s => s.ext.view.mesh.traverse(o => o.visible = false));
       viewer.render();
     },
     ShowSolids: () => {
-      cadRegistry.getAllShells().forEach(s => s.cadGroup.traverse(o => o.visible = true));
+      cadRegistry.getAllShells().forEach(s => s.ext.view.mesh.traverse(o => o.visible = true));
       viewer.render();
     },
     Clear: () => {

web/app/cad/model/mface.ts

@@ -23,7 +23,7 @@ export class MFace extends MObject {
   brepFace: any;
 
   assemblyNodes: {
-    // normal: AssemblyUnitVectorNode
+    normal: AssemblyUnitVectorNode
     plane: AssemblyPlaneNode,
     // w: AssemblyScalarNode
   };
@@ -43,7 +43,7 @@ export class MFace extends MObject {
     this.sketchLoops = [];
     this._csys = csys;
     this.assemblyNodes = {
-      // normal: new AssemblyUnitVectorNode(this, () => this.normal()),
+      normal: new AssemblyUnitVectorNode(this, () => this.normal()),
       // w: new AssemblyScalarNode(this, 'W', () => this.depth())
       plane: new AssemblyPlaneNode(this, () => this.normal(), () => this.depth())
     };

web/app/cad/model/mobject.ts

@@ -1,3 +1,5 @@
+import {AssemblyNode} from "../assembly/assembly";
+
 export abstract class MObject {
 
   TYPE: string;
@@ -5,6 +7,10 @@ export abstract class MObject {
   id: string;
   ext: any = {};
 
+  assemblyNodes?: {
+    [key: string]: AssemblyNode
+  };
+
   constructor(TYPE, id) {
     this.TYPE = TYPE;
     this.id = id;

web/app/cad/model/mshell.ts

@@ -6,6 +6,10 @@ import CSys from 'math/csys';
 import {Matrix3} from "math/l3space";
 import {state, StateStream} from "lstream";
 import {AssemblyCSysNode} from "../assembly/nodes/assemblyCSysNode";
+import {AssemblyOrientationNode} from "../assembly/nodes/assemblyOrientationNode";
+import {AssemblyVectorNode} from "../assembly/nodes/assemblyVectorNode";
+import {AssemblyTranslationNode} from "../assembly/nodes/assemblyTranslationNode";
+import {AssemblyLocationNode} from "../assembly/nodes/assemblyLocationNode";
 
 export class MShell extends MObject {
 
@@ -18,18 +22,20 @@ export class MShell extends MObject {
   edges = [];
   vertices = [];
 
-  location$: StateStream<CSys> = state(CSys.origin());
+  location$: StateStream<Matrix3> = state(new Matrix3());
-  locationMatrix$ = this.location$.map((csys: CSys) => csys.outTransformation).remember();
 
   assemblyNodes: {
-    location: AssemblyCSysNode
+    location: AssemblyLocationNode,
+    orientation: AssemblyOrientationNode,
+    translation: AssemblyTranslationNode,
   };
 
   constructor() {
     super(MShell.TYPE, MObjectIdGenerator.next(MShell.TYPE, 'S'));
     // @ts-ignore
     this.assemblyNodes = {
-      location: new AssemblyCSysNode( this, () => new Matrix3() )
+      location: new AssemblyLocationNode(this, () => new Matrix3() ),
+      orientation: new AssemblyOrientationNode( this, () => new Matrix3() )
     };
 
   }

web/app/cad/projectManager/projectManagerPlugin.ts

@@ -203,7 +203,7 @@ export interface ProjectModel {
 
   expressions: string
 
-  assembly?: AssemblyConstraintDefinition[][];
+  assembly?: AssemblyConstraintDefinition[];
 
 }
 

web/app/cad/scene/views/shellView.js

@@ -29,6 +29,7 @@ export class ShellView extends View {
     const geometry = new THREE.Geometry();
     geometry.dynamic = true;
     this.mesh = new SketchMesh(geometry, this.material);
+    // this.mesh.visible  = false;
     this.rootGroup.add(this.mesh);
     
 
@@ -48,7 +49,7 @@ export class ShellView extends View {
 
     this.rootGroup.matrixAutoUpdate = false;
 
-    this.model.locationMatrix$.attach(loc => {
+    this.model.location$.attach(loc => {
       loc.setToMatrix(this.rootGroup.matrix);
       this.rootGroup.matrixWorldNeedsUpdate = true;
       viewer.requestRender();

web/app/math/math.js

@@ -303,3 +303,8 @@ export function lineLineIntersection2d(p1, p2, v1, v2) {
 export const DEG_RAD = Math.PI / 180.0;
 
 export const sq = (a) => a * a;
+
+
+export function clamp(num, min, max) {
+  return Math.min(max, Math.max(num, min))
+}

web/app/math/optim.js

@@ -326,7 +326,7 @@ var dog_leg = function (subsys, rough) {
   var g_inf = n.norminf(g);
   var fx_inf = n.norminf(fx);
   
-  var iterLimit = rough ? 100 : 500;
+  var iterLimit = rough ? 100 : 50000;
   var divergenceLimit = 1e6 * (err + 1e6);
 
   var delta = 10;
@@ -339,8 +339,6 @@ var dog_leg = function (subsys, rough) {
 
     if (fx_inf <= tolf) {
       returnCode = SUCCESS;
-    } else if (g_inf <= tolg) {
-      returnCode = SUCCESS;
     } else if (iter >= iterLimit) {
       returnCode = ITER_LIMIT;
     } else if (delta <= tolx * (tolx + n.norm2(x))) {
@@ -442,7 +440,7 @@ var dog_leg = function (subsys, rough) {
     }
     //log.push([stepKind,err,  delta,rho]);
 
-    if (acceptCandidate) {
+    const step = () => {
       x = n.clone(x_new);
       J = n.clone(J_new);
       fx = n.clone(fx_new);
@@ -453,12 +451,25 @@ var dog_leg = function (subsys, rough) {
       // get infinity norms
       g_inf = n.norminf(g);
       fx_inf = n.norminf(fx);
+    };
+
+    if (acceptCandidate) {
+      step();
+    }
+
+    if (g_inf <= tolg) {
+      for (let i = 0; i < x_new.length; ++i) {
+        x_new[i] += 1;
+      }
+      err_new = subsys.calcResidual(fx_new);
+      step();
     }
 
     iter++;
   }
   //log.push(returnCode);
   //window.___log(log);
+  console.log("DOGLE: " +  iter)
   return _result(iter, err, returnCode);
 };
 

web/app/sketcher/constr/solver.js

@@ -246,6 +246,7 @@ var prepare = function(constrs, locked) {
     //if (simpleMode) return nullResult;
     if (constrs.length === 0) return nullResult;
     if (sys.params.length === 0) return nullResult;
+    // return solve_lm(sys, model, jacobian, rough);
     switch (alg) {
       case 2:
         return solve_lm(sys, model, jacobian, rough);
@@ -274,7 +275,7 @@ var prepare = function(constrs, locked) {
 
 var solve_lm = function(sys, model, jacobian, rough) {
   var opt = new LMOptimizer(sys.getParams(), newVector(sys.constraints.length), model, jacobian);
-  opt.evalMaximalCount = 100 * sys.params.length;
+  opt.evalMaximalCount = 100000; //100 * sys.params.length;
   var eps = rough ? 0.001 : 0.00000001;
   opt.init0(eps, eps, eps);
   var returnCode = 1;
@@ -283,10 +284,19 @@ var solve_lm = function(sys, model, jacobian, rough) {
   } catch (e) {
     returnCode = 2;
   }
+  if (returnCode === 1) {
     sys.setParams(res[0]);
+  }
+  console.log("LM result: ")
+  console.log({
+    evalCount : opt.evalCount,
+    error : sys.error(),
+  });
+
   return {
     evalCount : opt.evalCount,
     error : sys.error(),
+    success: returnCode === 1 && sys.error() < 1e-3,
     returnCode : returnCode
   };
 };