Examples with Point artisynth.core.mechmodels.Point used on opensource projects

Example 6 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class FemElement3d method getNaturalCoordinatesGSS.

/**
 * Given point p, get its natural coordinates with respect to this element.
 * Returns a positive number if the algorithm converges, or -1 if a maximum
 * number of iterations has been reached. Uses a modified Newton's method to solve the
 * equations. The <code>coords</code> argument that returned the coordinates is
 * used, on input, to supply an initial guess of the coordinates.
 * Zero is generally a safe guess.
 *
 * @param coords
 * Outputs the natural coordinates, and supplies (on input) an initial
 * guess as to their position.
 * @param pnt
 * A given point (in world coords)
 * @param maxIters
 * Maximum number of Newton iterations
 * @return the number of iterations required for convergence, or
 * -1 if the calculation did not converge.
 */
public int getNaturalCoordinatesGSS(Vector3d coords, Point3d pnt, int maxIters) {
    if (!coordsAreInside(coords)) {
        // if not inside, reset coords to 0
        coords.setZero();
    }
    // if FEM is frame-relative, transform to local coords
    Point3d lpnt = new Point3d(pnt);
    if (getGrandParent() instanceof FemModel3d) {
        FemModel3d fem = (FemModel3d) getGrandParent();
        if (fem.isFrameRelative()) {
            lpnt.inverseTransform(fem.getFrame().getPose());
        }
    }
    Vector3d res = new Point3d();
    int i;
    double tol = RenderableUtils.getRadius(this) * 1e-12;
    computeNaturalCoordsResidual(res, coords, lpnt);
    double prn = res.norm();
    // System.out.println ("res=" + prn);
    if (prn < tol) {
        // already have the right answer
        return 0;
    }
    LUDecomposition LUD = new LUDecomposition();
    Point3d prevCoords = new Point3d();
    Vector3d dNds = new Vector3d();
    Matrix3d dxds = new Matrix3d();
    Vector3d del = new Point3d();
    GSSResidual func = new GSSResidual(this, lpnt);
    /*
       * solve using Newton's method.
       */
    for (i = 0; i < maxIters; i++) {
        // compute the Jacobian dx/ds for the current guess
        dxds.setZero();
        for (int k = 0; k < numNodes(); k++) {
            getdNds(dNds, k, coords);
            dxds.addOuterProduct(myNodes[k].getLocalPosition(), dNds);
        }
        LUD.factor(dxds);
        double cond = LUD.conditionEstimate(dxds);
        if (cond > 1e10)
            System.err.println("Warning: condition number for solving natural coordinates is " + cond);
        // solve Jacobian to obtain an update for the coords
        LUD.solve(del, res);
        del.negate();
        // use an absolute value for a tolerance threshold
        if (del.norm() < 1e-10) {
            // System.out.println ("1 res=" + res.norm());
            return i + 1;
        }
        prevCoords.set(coords);
        coords.add(del);
        computeNaturalCoordsResidual(res, coords, lpnt);
        double rn = res.norm();
        // If the residual norm is within tolerance, we have converged.
        if (rn < tol) {
            // System.out.println ("2 res=" + rn);
            return i + 1;
        }
        // do a golden section search in range
        double eps = 1e-12;
        func.set(prevCoords, del);
        double alpha = GoldenSectionSearch.minimize(func, 0, 1, eps, 0.8 * prn * prn);
        coords.scaledAdd(alpha, del, prevCoords);
        computeNaturalCoordsResidual(res, coords, lpnt);
        rn = res.norm();
        // System.out.println (" alpha=" + alpha + " rn=" + rn + " prn=" + prn);
        if (alpha < eps) {
            // failed
            return -1;
        }
        prn = rn;
    }
    // failed
    return -1;
}

Example 7 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class FemElement3d method getNaturalCoordinates.

/**
 * Given point p, get its natural coordinates with respect to this element.
 * Returns a positive number if the algorithm converges, or -1 if a maximum
 * number of iterations has been reached. Uses a modified Newton's method to solve the
 * equations. The <code>coords</code> argument that returned the coordinates is
 * used, on input, to supply an initial guess of the coordinates.
 * Zero is generally a safe guess.
 *
 * @param coords
 * Outputs the natural coordinates, and supplies (on input) an initial
 * guess as to their position.
 * @param pnt
 * A given point (in world coords)
 * @param maxIters
 * Maximum number of Newton iterations
 * @return the number of iterations required for convergence, or
 * -1 if the calculation did not converge.
 */
public int getNaturalCoordinates(Vector3d coords, Point3d pnt, int maxIters) {
    if (!coordsAreInside(coords)) {
        // if not inside, reset coords to 0
        coords.setZero();
    }
    // if FEM is frame-relative, transform to local coords
    Point3d lpnt = new Point3d(pnt);
    if (getGrandParent() instanceof FemModel3d) {
        FemModel3d fem = (FemModel3d) getGrandParent();
        if (fem.isFrameRelative()) {
            lpnt.inverseTransform(fem.getFrame().getPose());
        }
    }
    Vector3d res = new Point3d();
    int i;
    double tol = RenderableUtils.getRadius(this) * 1e-12;
    computeNaturalCoordsResidual(res, coords, lpnt);
    double prn = res.norm();
    // System.out.println ("res=" + prn);
    if (prn < tol) {
        // already have the right answer
        return 0;
    }
    LUDecomposition LUD = new LUDecomposition();
    Vector3d prevCoords = new Vector3d();
    Vector3d dNds = new Vector3d();
    Matrix3d dxds = new Matrix3d();
    Vector3d del = new Point3d();
    /*
       * solve using Newton's method.
       */
    for (i = 0; i < maxIters; i++) {
        // compute the Jacobian dx/ds for the current guess
        dxds.setZero();
        for (int k = 0; k < numNodes(); k++) {
            getdNds(dNds, k, coords);
            dxds.addOuterProduct(myNodes[k].getLocalPosition(), dNds);
        }
        LUD.factor(dxds);
        double cond = LUD.conditionEstimate(dxds);
        if (cond > 1e10)
            System.err.println("Warning: condition number for solving natural coordinates is " + cond);
        // solve Jacobian to obtain an update for the coords
        LUD.solve(del, res);
        // use an absolute value for a tolerance threshold
        if (del.norm() < 1e-10) {
            // System.out.println ("1 res=" + res.norm());
            return i + 1;
        }
        prevCoords.set(coords);
        coords.sub(del);
        computeNaturalCoordsResidual(res, coords, lpnt);
        double rn = res.norm();
        // If the residual norm is within tolerance, we have converged.
        if (rn < tol) {
            // System.out.println ("2 res=" + rn);
            return i + 1;
        }
        if (rn > prn) {
            // it may be that "coords + del" is a worse solution.  Let's make
            // sure we go the correct way binary search suitable alpha in [0 1]
            double eps = 1e-12;
            // start by limiting del to a magnitude of 1
            if (del.norm() > 1) {
                del.normalize();
            }
            // and keep cutting the step size in half until the residual starts
            // dropping again
            double alpha = 0.5;
            while (alpha > eps && rn > prn) {
                coords.scaledAdd(-alpha, del, prevCoords);
                computeNaturalCoordsResidual(res, coords, lpnt);
                rn = res.norm();
                alpha *= 0.5;
            // System.out.println ("  alpha=" + alpha + " rn=" + rn);
            }
            // System.out.println (" alpha=" + alpha + " rn=" + rn + " prn=" + prn);
            if (alpha < eps) {
                // failed
                return -1;
            }
        }
        prn = rn;
    }
    // failed
    return -1;
}

Example 8 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class FemMeshComp method createPointAttachment.

public PointFem3dAttachment createPointAttachment(Point pnt) {
    if (!(getMesh() instanceof PolygonalMesh)) {
        return null;
    }
    PolygonalMesh mesh = (PolygonalMesh) getMesh();
    if (!mesh.isTriangular()) {
        return null;
    }
    // Find nearest face to the point. The vertices of this face will be used
    // to find the nodes and weight for the attachment.
    BVFeatureQuery query = new BVFeatureQuery();
    Point3d near = new Point3d();
    Vector2d uv = new Vector2d();
    Face face = query.nearestFaceToPoint(near, uv, mesh, pnt.getPosition());
    Vertex3d[] vtxs = face.getTriVertices();
    double[] wgts = new double[] { 1 - uv.x - uv.y, uv.x, uv.y };
    HashMap<FemNode, Double> nodeWeights = new HashMap<FemNode, Double>();
    for (int i = 0; i < vtxs.length; i++) {
        PointAttachment va = myVertexAttachments.get(vtxs[i].getIndex());
        if (va instanceof PointParticleAttachment) {
            PointParticleAttachment ppa = (PointParticleAttachment) va;
            FemNode node = (FemNode) ppa.getParticle();
            accumulateNodeWeights(node, wgts[i], nodeWeights);
        } else if (va instanceof PointFem3dAttachment) {
            PointFem3dAttachment pfa = (PointFem3dAttachment) va;
            for (int k = 0; k < pfa.numMasters(); k++) {
                FemNode node = pfa.getNodes()[k];
                double w = pfa.getCoordinate(k);
                accumulateNodeWeights(node, w * wgts[i], nodeWeights);
            }
        }
    }
    // Create a new PointFem3dAttachment
    PointFem3dAttachment ax = new PointFem3dAttachment(pnt);
    VectorNd weightVec = new VectorNd();
    for (Double d : nodeWeights.values()) {
        weightVec.append(d);
    }
    ax.setFromNodes(nodeWeights.keySet(), weightVec);
    return ax;
}

Example 9 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class PointSkinAttachment method getSoftReferences.

@Override
public void getSoftReferences(List<ModelComponent> refs) {
    super.getSoftReferences(refs);
    Point point = getPoint();
    if (point != null) {
        refs.add(point);
    }
    for (int i = 0; i < myNumConnections; i++) {
        ModelComponent m = myConnections[i].getMaster();
        if (m != null) {
            refs.add(myConnections[i].getMaster());
        }
    }
}

Example 10 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class InverseManager method configureTargetMotionProbe.

private void configureTargetMotionProbe(NumericProbeBase probe, ArrayList<MotionTargetComponent> targets, String filename) {
    // System.out.println ("configuring motion probe");
    ArrayList<Property> props = new ArrayList<Property>();
    for (ModelComponent target : targets) {
        if (target instanceof Point) {
            props.add(target.getProperty("position"));
        } else if (target instanceof Frame) {
            props.add(target.getProperty("position"));
            props.add(target.getProperty("orientation"));
        } else {
            System.err.println("Unknown target component type: " + target.getClass().toString());
        }
    }
    // probe.setModel(myController.getMech());
    probe.setAttachedFileName(filename);
    if (probe instanceof NumericInputProbe) {
        ((NumericInputProbe) probe).setInputProperties(props.toArray(new Property[props.size()]));
    } else if (probe instanceof NumericOutputProbe) {
        ((NumericOutputProbe) probe).setOutputProperties(props.toArray(new Property[props.size()]));
    }
    if (probe instanceof NumericInputProbe) {
        File file = probe.getAttachedFile();
        if (file == null || !file.exists()) {
            ((NumericInputProbe) probe).loadEmpty();
            probe.setActive(false);
        } else {
            try {
                probe.load();
                probe.setActive(true);
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }
}