diff --git a/modules/math/l3space.ts b/modules/math/l3space.ts index 207f851f..5e6e5a33 100644 --- a/modules/math/l3space.ts +++ b/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; + } } diff --git a/modules/plugable/index.ts b/modules/plugable/index.ts new file mode 100644 index 00000000..7ac7710d --- /dev/null +++ b/modules/plugable/index.ts @@ -0,0 +1,14 @@ + +export interface Plugin { + + id: string; + + dependencies: string[]; + + activate(ctx: InContext & OutContext): () => void | void; + +} + +export function activatePlugins(plugins: Plugin[], context: any) { + +} \ No newline at end of file diff --git a/web/app/cad/assembly/assembly.ts b/web/app/cad/assembly/assembly.ts index 5dfd2def..bb42ab16 100644 --- a/web/app/cad/assembly/assembly.ts +++ b/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 []; } diff --git a/web/app/cad/assembly/assemblyConstraintDefinition.ts b/web/app/cad/assembly/assemblyConstraintDefinition.ts index 49b29c88..ce80390b 100644 --- a/web/app/cad/assembly/assemblyConstraintDefinition.ts +++ b/web/app/cad/assembly/assemblyConstraintDefinition.ts @@ -7,4 +7,4 @@ export interface AssemblyConstraintDefinition { objects: string[]; constants: ConstantsDefinitions -} \ No newline at end of file +} diff --git a/web/app/cad/assembly/assemblyPlugin.ts b/web/app/cad/assembly/assemblyPlugin.ts index 7ecc926e..54b65fb9 100644 --- a/web/app/cad/assembly/assemblyPlugin.ts +++ b/web/app/cad/assembly/assemblyPlugin.ts @@ -2,44 +2,46 @@ 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 {Constraints3D, createAssemblyConstraint} from "./constraints3d"; +import {AssemblyProcess, launchAssembly} from "./assemblySolver"; 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([]); + const constraints$ = state([]); const status$ = state(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 => { - if (stages.length === 0) { - stages.push([]) - } - stages[stages.length - 1].push({ + constraints$.mutate(constraints => { + constraints.push({ typeId, objects, constants }); }) } function removeConstraint(constr: AssemblyConstraintDefinition) { - constraints$.mutate(stages => { - for (let constrs of stages) { - const index = constrs.indexOf(constr); - if (index !== -1) { - constrs.splice(index, 1); - } + constraints$.mutate(constrs => { + 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 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 constraints = constraints$.value; - 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; + constraints$: StateStream; status$: StateStream; @@ -103,9 +91,9 @@ export interface AssemblyService { solveAssembly(): void; - loadConstraints(constraints: AssemblyConstraintDefinition[][]); + loadConstraints(constraints: AssemblyConstraintDefinition[]); - getConstraints(): AssemblyConstraintDefinition[][]; + getConstraints(): AssemblyConstraintDefinition[]; } diff --git a/web/app/cad/assembly/assemblySolver.ts b/web/app/cad/assembly/assemblySolver.ts index c80b116a..3c5d07d4 100644 --- a/web/app/cad/assembly/assemblySolver.ts +++ b/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 - const constraints = [].concat(...stages); + model: MObject; + relationships = new Map(); - const objects = new Set(); - constraints.forEach(c => c.objects.forEach(o => objects.add(o))); + reset() { + this.model.traverse(m => { + if (m.assemblyNodes) { + Object.values(m.assemblyNodes).forEach((node: AssemblyNode) => node.reset()); + } + }) + } +} - const stage: ISolveStage = { - objects: objects, - index: 0 +export class AssemblyConstraint { + objects: MObject[] = []; + schema: AssemblyConstraintSchema; +} + +export class AssemblyProcess { + + queue: RigidBody[]; + solved: Set = new Set(); + solveStatus: SolveStatus = { + success: true, + error: 0 }; + errorStep = null; - const roots = new Set(); - objects.forEach(o => { - const root = o.model.root; - if (root instanceof MShell) { - roots.add(root); - objects.add(root.assemblyNodes.location) + constructor(cadRegistry: CadRegistry, constraintDefs: AssemblyConstraintDefinition[]) { + this.queue = buildAssemblyQueue(cadRegistry, constraintDefs) + } + + step() { + + 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(); + 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(); + 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 => { - o.stage = stage; - o.reset(); + return Array.from(bodies.values()); +} + +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(), + index: 0 + }; + + const stage: ISolveStage = { + objects: new Set(), + 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)); - objects.forEach(assemblyNode => { + solvingConstraints.forEach(c => { + 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 => { - return targetCsys; - }); + // shell.location$.update(csys => { + // return targetCsys; + // }); // shell.location$.mutate(csys => { // csys.x = basis[0]; // csys.y = basis[1]; @@ -107,4 +276,5 @@ function applyResults(shell: MShell, targetCsysParams: AssemblyCSysNode): void { // csys.origin = new Vector(ox, oy, oz)._minus(shell.csys.origin); // }); -} \ No newline at end of file +} + diff --git a/web/app/cad/assembly/constraints3d.ts b/web/app/cad/assembly/constraints3d.ts index f725f7b1..cb456233 100644 --- a/web/app/cad/assembly/constraints3d.ts +++ b/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 {Circle} from "../../sketcher/shapes/circle"; -import {Arc} from "../../sketcher/shapes/arc"; +import {IconType} from "react-icons"; +import Vector from "math/vector"; export const Constraints3D = { - FaceParallel: { - id: 'FaceParallel', - name: 'Face Parallel', + PlaneOppositeNormals: { + id: 'PlaneOppositeNormals', + 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); - plane2.visitParams(cb); + cb(plane1.theta); + 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) - .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) - - ); - polynomials.push( - new Polynomial() - .monomial() + new Polynomial(0) + .monomial(1) .term(w1, POW_1_FN) .monomial() .term(w2, POW_1_FN) + ); - } - }, UnitVectorConsistency: { @@ -224,75 +213,181 @@ export const Constraints3D = { }, }, - RigidBodyPlaneLink: { - id: 'RigidBodyPlaneLink', - name: 'RigidBodyPlaneLink', + PlaneNormalLink: { + id: 'PlaneNormalLink', + name: 'Plane Normal Link', icon: NoIcon, - defineParamsScope: ([csys, plane], cb) => { - csys.visitParams(cb); - plane.visitParams(cb); + defineParamsScope: ([location, plane], cb) => { + cb(location.alpha); + 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 wStar = plane.getDepth(); + const {x: nStarX, y: nStarY, z: nStarZ} = plane.getNormal(); - 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) - .monomial(-1) - .term(x, POW_1_FN) - .term(w, POW_1_FN) - .monomial(xStar) - .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) + new Polynomial(-xP * w0) + .monomial(xP) + .term(w, POW_1_FN) + .monomial(-1) + .term(ox, POW_1_FN) ); polynomials.push( - new Polynomial(0) - .monomial(-1) - .term(y, POW_1_FN) + new Polynomial(-yP * w0) + .monomial(yP) .term(w, POW_1_FN) - .monomial(xStar) - .term(iy, POW_1_FN) - .monomial(yStar) - .term(jy, POW_1_FN) - .monomial(zStar) - .term(ky, POW_1_FN) - .monomial() + .monomial(-1) .term(oy, POW_1_FN) ); polynomials.push( - new Polynomial(0) - .monomial(-1) - .term(z, POW_1_FN) + new Polynomial(-zP * w0) + .monomial(zP) .term(w, POW_1_FN) - .monomial(xStar) - .term(iz, POW_1_FN) - .monomial(yStar) - .term(jz, POW_1_FN) - .monomial(zStar) - .term(kz, POW_1_FN) - .monomial() + .monomial(-1) .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); -} \ No newline at end of file diff --git a/web/app/cad/assembly/nodes/assemblyCSysNode.ts b/web/app/cad/assembly/nodes/assemblyCSysNode.ts index f1f98045..68889c10 100644 --- a/web/app/cad/assembly/nodes/assemblyCSysNode.ts +++ b/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]) diff --git a/web/app/cad/assembly/nodes/assemblyLocationNode.ts b/web/app/cad/assembly/nodes/assemblyLocationNode.ts new file mode 100644 index 00000000..d6710b0a --- /dev/null +++ b/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]) + ]; + } + +} \ No newline at end of file diff --git a/web/app/cad/assembly/nodes/assemblyOrientationNode.ts b/web/app/cad/assembly/nodes/assemblyOrientationNode.ts new file mode 100644 index 00000000..4874254a --- /dev/null +++ b/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]) + ]; + } + +} \ No newline at end of file diff --git a/web/app/cad/assembly/nodes/assemblyPlaneNode.ts b/web/app/cad/assembly/nodes/assemblyPlaneNode.ts index 4022507d..ab069a27 100644 --- a/web/app/cad/assembly/nodes/assemblyPlaneNode.ts +++ b/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'); - y = new Param(0, 'Y'); - z = new Param(0, 'Z'); + theta = new Param(0, 'T'); + phi = new Param(0, 'P'); 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.y); - cb(this.z); + cb(this.theta); + cb(this.phi); cb(this.w); } reset() { const {x, y, z} = this.getNormal(); const w = this.getDepth(); - this.x.set(x); - this.y.set(y); - this.z.set(z); + const phi = Math.atan2(y, x); + const theta = Math.acos(clamp(z, -1, 1)); + + 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) { - return [ - new AlgNumConstraint(Constraints3D.RigidBodyPlaneLink, [body, this]) - ]; + createOrientationRelationship(location: AssemblyLocationNode): AlgNumConstraint[] { + return [new AlgNumConstraint(Constraints3D.PlaneNormalLink, [location, this])]; + } + + createTranslationRelationship(location: AssemblyLocationNode): AlgNumConstraint[] { + return [new AlgNumConstraint(Constraints3D.PlaneDepthLink, [location, this])]; } } \ No newline at end of file diff --git a/web/app/cad/assembly/nodes/assemblyScalarNode.ts b/web/app/cad/assembly/nodes/assemblyScalarNode.ts index 20c60173..f8ee7f18 100644 --- a/web/app/cad/assembly/nodes/assemblyScalarNode.ts +++ b/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]) - ]; - } - } \ No newline at end of file diff --git a/web/app/cad/assembly/nodes/assemblyTranslationNode.ts b/web/app/cad/assembly/nodes/assemblyTranslationNode.ts new file mode 100644 index 00000000..3136e51b --- /dev/null +++ b/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 [ + + ]; + } + +} \ No newline at end of file diff --git a/web/app/cad/assembly/nodes/assemblyUnitVectorNode.ts b/web/app/cad/assembly/nodes/assemblyUnitVectorNode.ts index c4370daa..bfffd4e9 100644 --- a/web/app/cad/assembly/nodes/assemblyUnitVectorNode.ts +++ b/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) { - return [ - // new AlgNumConstraint(Constraints3D.RigidBodyLink3x3, [body, this]) - ]; + createRigidBodyOrientationRelationship(orientationNode: AssemblyOrientationNode): AlgNumConstraint[] { + return [new AlgNumConstraint(Constraints3D.RigidBodyLink3x3, [orientationNode, this])]; } + } \ No newline at end of file diff --git a/web/app/cad/assembly/ui/AssemblyView.tsx b/web/app/cad/assembly/ui/AssemblyView.tsx index 8b1ca1cc..16b54e5c 100644 --- a/web/app/cad/assembly/ui/AssemblyView.tsx +++ b/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() {
Status:
- {constraints.map((stage, i) => - {stage.map((constr, j) => ) } - )} + {constraints.map((constr, i) => )} + } @@ -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
Invalid Constraint {c.typeId}
} diff --git a/web/app/cad/assembly/ui/ModellerContextualActions.tsx b/web/app/cad/assembly/ui/ModellerContextualActions.tsx index fb6c8a3f..c4fd4927 100644 --- a/web/app/cad/assembly/ui/ModellerContextualActions.tsx +++ b/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) { diff --git a/web/app/cad/assembly/ui/StepByStepSimulation.tsx b/web/app/cad/assembly/ui/StepByStepSimulation.tsx new file mode 100644 index 00000000..baaca170 --- /dev/null +++ b/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(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 + +} diff --git a/web/app/cad/debugPlugin.js b/web/app/cad/debugPlugin.js index 818e829e..5df3d98a 100644 --- a/web/app/cad/debugPlugin.js +++ b/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: () => { diff --git a/web/app/cad/model/mface.ts b/web/app/cad/model/mface.ts index dbd4eb35..794fa319 100644 --- a/web/app/cad/model/mface.ts +++ b/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()) }; diff --git a/web/app/cad/model/mobject.ts b/web/app/cad/model/mobject.ts index f2ba9493..3b7921c2 100644 --- a/web/app/cad/model/mobject.ts +++ b/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; diff --git a/web/app/cad/model/mshell.ts b/web/app/cad/model/mshell.ts index 7461e17f..744eaf0c 100644 --- a/web/app/cad/model/mshell.ts +++ b/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 = state(CSys.origin()); - locationMatrix$ = this.location$.map((csys: CSys) => csys.outTransformation).remember(); + location$: StateStream = state(new Matrix3()); 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() ) }; } diff --git a/web/app/cad/projectManager/projectManagerPlugin.ts b/web/app/cad/projectManager/projectManagerPlugin.ts index 74ea6f53..ee9ce02a 100644 --- a/web/app/cad/projectManager/projectManagerPlugin.ts +++ b/web/app/cad/projectManager/projectManagerPlugin.ts @@ -203,7 +203,7 @@ export interface ProjectModel { expressions: string - assembly?: AssemblyConstraintDefinition[][]; + assembly?: AssemblyConstraintDefinition[]; } diff --git a/web/app/cad/scene/views/shellView.js b/web/app/cad/scene/views/shellView.js index 356f8d49..44d3b261 100644 --- a/web/app/cad/scene/views/shellView.js +++ b/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(); diff --git a/web/app/math/math.js b/web/app/math/math.js index 8ffc7376..cac70a46 100644 --- a/web/app/math/math.js +++ b/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)) +} diff --git a/web/app/math/optim.js b/web/app/math/optim.js index efbabb8d..3ebcbb19 100644 --- a/web/app/math/optim.js +++ b/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); }; diff --git a/web/app/sketcher/constr/solver.js b/web/app/sketcher/constr/solver.js index 37ebefa3..355950ca 100644 --- a/web/app/sketcher/constr/solver.js +++ b/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; } - sys.setParams(res[0]); + 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 }; };