Examples with Matrix3d maspack.matrix.Matrix3d used on opensource projects

Example 26 with Matrix3d

use of maspack.matrix.Matrix3d in project artisynth_core by artisynth.

the class MFreeMuscleModel method renderElementDirection.

protected void renderElementDirection(Renderer renderer, RenderProps props, FemElement3d elem, float[] coords0, float[] coords1, Matrix3d F, Vector3d dir, double len) {
    IntegrationData3d[] idata = elem.getIntegrationData();
    elem.computeRenderCoordsAndGradient(F, coords0);
    int ndirs = 0;
    dir.setZero();
    for (int i = 0; i < idata.length; i++) {
        Matrix3d Frame = idata[i].getFrame();
        if (Frame != null) {
            dir.x += Frame.m00;
            dir.y += Frame.m10;
            dir.z += Frame.m20;
            ndirs++;
        }
    }
    if (ndirs > 0) {
        dir.normalize();
        F.mul(dir, dir);
        dir.scale(len);
        coords0[0] -= (float) dir.x / 2;
        coords0[1] -= (float) dir.y / 2;
        coords0[2] -= (float) dir.z / 2;
        coords1[0] = coords0[0] + (float) dir.x;
        coords1[1] = coords0[1] + (float) dir.y;
        coords1[2] = coords0[2] + (float) dir.z;
        props.getLineColor(myDirectionColor);
        renderer.drawLine(props, coords0, coords1, myDirectionColor, /*capped=*/
        false, /*highlight=*/
        false);
    }
}

Example 27 with Matrix3d

use of maspack.matrix.Matrix3d in project artisynth_core by artisynth.

the class MFreeMuscleModel method computeAverageFiberDirection.

public static boolean computeAverageFiberDirection(Vector3d dir, Point3d pos, double rad, PolylineMesh mesh) {
    BVTree bvh = mesh.getBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    Matrix3d cov = new Matrix3d();
    SVDecomposition3d svd = new SVDecomposition3d();
    Vector3d tmp = new Vector3d();
    Matrix3d tmp2 = new Matrix3d();
    bvh.intersectSphere(nodes, pos, rad);
    dir.setZero();
    int nsegs = 0;
    // for computing sign of direction vector
    Vector3d segmentSum = new Vector3d();
    // System.out.println("p=[");
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            LineSegment seg = (LineSegment) elements[i];
            seg = getSegmentInsideSphere(seg, pos, rad);
            if (seg != null) {
                tmp.sub(seg.myVtx1.pnt, seg.myVtx0.pnt);
                if (tmp.norm() >= 1e-8 * rad) {
                    // System.out.println(seg.myVtx0.getPosition() + " " +
                    // seg.myVtx1.getPosition());
                    nsegs++;
                    // prepare to average directions using SVD
                    computeCov(tmp2, tmp);
                    cov.add(tmp2);
                    segmentSum.add(tmp);
                }
            }
        }
    }
    if (nsegs > 0) {
        // we are technically including both +/- directions, so
        // we have twice the number of points
        cov.scale(2.0 / (2.0 * nsegs - 1));
        try {
            svd.factor(cov);
        } catch (Exception e) {
        // System.err.println(e.getMessage());
        }
        // principal components
        tmp2 = svd.getU();
        tmp2.getColumn(0, dir);
        dir.normalize();
        // most line segments
        if (dir.dot(segmentSum) < 0) {
            dir.scale(-1);
        }
        return true;
    } else {
        return false;
    }
}

Example 28 with Matrix3d

use of maspack.matrix.Matrix3d in project artisynth_core by artisynth.

the class AffineNumericInputProbe method getTransform.

public AffineTransform3d getTransform() {
    Matrix3d Ar = new Matrix3d();
    R.getSubMatrix(0, 0, Ar);
    // not very
    Vector3d At = new Vector3d(t.get(0), t.get(1), t.get(2));
    return new AffineTransform3d(Ar, At);
}

Example 29 with Matrix3d

use of maspack.matrix.Matrix3d in project artisynth_core by artisynth.

the class RigidTransformInputProbe method addTransform.

public void addTransform(double t, RigidTransform3d tx) {
    VectorNd posVector = new VectorNd();
    Vector3d offset = tx.getOffset();
    posVector.append(offset.x);
    posVector.append(offset.y);
    posVector.append(offset.z);
    RotationMatrix3d rot = new RotationMatrix3d();
    Vector3d scale = new Vector3d();
    Matrix3d shear = new Matrix3d();
    tx.getMatrixComponents(rot, scale, shear);
    Quaternion q = new Quaternion(rot.getAxisAngle());
    posVector.append(q.s);
    posVector.append(q.u.x);
    posVector.append(q.u.y);
    posVector.append(q.u.z);
    NumericListKnot knot = new NumericListKnot(myVectorSize);
    knot.t = t;
    knot.v.set(posVector);
    myTransAndQuaternParams.add(knot);
    if (t < getStartTime()) {
        setStartTime(t);
    }
    if (t > getStopTime()) {
        setStopTime(t);
    }
}

Example 30 with Matrix3d

use of maspack.matrix.Matrix3d in project artisynth_core by artisynth.

the class MatricesUBO method updateMatrices.

public void updateMatrices(GL3 gl, Matrix4d proj, RigidTransform3d view, AffineTransform3dBase model, AffineTransform2dBase texture) {
    ByteBuffer buff = getBuffer();
    // model view
    AffineTransform3d modelview = new AffineTransform3d(view);
    modelview.mul(model);
    // pvm matrix
    buff.position(getByteOffset(PVM_IDX));
    mul(proj, modelview, buff);
    // model-view
    buff.position(getByteOffset(VM_IDX));
    putMatrix(buff, modelview);
    // model matrix
    buff.position(getByteOffset(M_IDX));
    putMatrix(buff, model);
    // normal
    buff.position(getByteOffset(NORMAL_IDX));
    if (model instanceof RigidTransform3d) {
        putMatrix(buff, modelview);
    } else {
        // invert model matrix
        Matrix3d A = new Matrix3d(model.getMatrix());
        if (!A.invert()) {
            // deal with non-invertible
            SVDecomposition3d svd3 = new SVDecomposition3d(model.getMatrix());
            svd3.pseudoInverse(A);
        }
        mulTranspose4x4(view.R, A, buff);
    }
    buff.position(getByteOffset(TEXTURE_IDX));
    putMatrix(buff, texture);
    buff.flip();
    update(gl, buff);
}