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

Example 66 with Vector3d

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

the class PolygonalMeshRenderer method updateFaceNormals.

protected void updateFaceNormals(RenderObject r, PolygonalMesh mesh) {
    boolean useRenderData = mesh.isRenderBuffered() && !mesh.isFixed();
    updateFaceNormals(mesh);
    ArrayList<Face> faces = mesh.getFaces();
    for (int i = 0; i < faces.size(); i++) {
        Vector3d nrm;
        if (useRenderData) {
            nrm = faces.get(i).getRenderNormal();
        } else {
            nrm = faces.get(i).getNormal();
        }
        r.setNormal(i, (float) nrm.x, (float) nrm.y, (float) nrm.z);
    }
}

Example 67 with Vector3d

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

the class OBB method setTransform.

/**
 * Sets the transform for this OBB from a covariance matrix and centroid.
 */
private void setTransform(Matrix3d C, Point3d cent) {
    SymmetricMatrix3d Csym = new SymmetricMatrix3d(C.m00, C.m11, C.m22, C.m01, C.m02, C.m12);
    Matrix3d U = new Matrix3d();
    Vector3d sig = new Vector3d();
    Matrix3d V = new Matrix3d();
    // Might want to do this factorization with SVDecomposition3d, but
    // SymmetricMatrix3d.getSVD() is a bit faster
    Csym.getSVD(U, sig, V);
    if (U.determinant() < 0) {
        U.m02 = -U.m02;
        U.m12 = -U.m12;
        U.m22 = -U.m22;
    }
    // Code to handle degenetrate eigenvalues
    // Handle degeneracies corresponding to equal-length axes in the
    // inertia ellipsoid. If any two axes have similar lengths, then
    // U is redefined solely using the direction of the remaining
    // axis.
    // 
    // In determining whether axes have similar lengths, we use
    // the fact that the moment of inertia of an ellipsoid
    // with unit mass is given by
    // 
    // sig.x = 1/5 (b^2 + c^2)
    // sig.y = 1/5 (a^2 + c^2)
    // sig.z = 1/5 (a^2 + b^2)
    // 
    // along with the fact that if a and b are close together, then
    // 
    // a^2 - b^2 ~= 2 b (a-b)
    // 
    // a^2 + b^2 ~= 2 a^2 ~= 2 b^2
    // 
    // and so
    // 
    // (a - b) ~= (a^2 - b^2) / 2 b
    // ~= (a^2 - b^2) / (sqrt(2) * sqrt(2 b^2))
    // ~= (a^2 - b^2) / (sqrt(2) * sqrt(a^2 + b^2))
    boolean xout = false, yout = false, zout = false;
    sig.scale(5);
    double tol = sig.infinityNorm() * 1e-6;
    if (Math.abs(sig.x - sig.y) / Math.sqrt(2 * sig.z) < tol) {
        // eliminate x and y
        xout = yout = true;
    }
    if (Math.abs(sig.x - sig.z) / Math.sqrt(2 * sig.y) < tol) {
        // eliminate x and z
        xout = zout = true;
    }
    if (Math.abs(sig.z - sig.y) / Math.sqrt(2 * sig.x) < tol) {
        // eliminate z and y
        zout = yout = true;
    }
    if (zout && xout && yout) {
        U.setIdentity();
    } else if (zout || yout || xout) {
        Vector3d zdir = new Vector3d();
        RotationMatrix3d R = new RotationMatrix3d();
        if (xout && yout) {
            U.getColumn(2, zdir);
        } else if (xout && zout) {
            U.getColumn(1, zdir);
        } else if (zout && yout) {
            U.getColumn(0, zdir);
        }
        R.setZDirection(zdir);
        U.set(R);
    }
    myX.R.set(U);
    myX.p.set(cent);
}

Example 68 with Vector3d

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

the class OBB method computeBoundsFromConvexHullPoints.

// protected void setUsingConvexHullOfPoints (
// double[] pnts, int npnts, double margin) {
// 
// Matrix3d C = new Matrix3d();
// Point3d cent = new Point3d();
// 
// quickhull3d.Point3d[] hullPnts =
// computeConvexHullAndCovariance (C, cent, pnts, npnts);
// 
// setTransform (C, cent);
// Point3d max = new Point3d (-INF, -INF, -INF);
// Point3d min = new Point3d (INF, INF, INF);
// computeBoundsFromConvexHullPoints (min, max, hullPnts, hullPnts.length);
// setWidthsAndCenter (min, max, margin);
// }
// 
private void computeBoundsFromConvexHullPoints(Point3d min, Point3d max, quickhull3d.Point3d[] pnts, int num) {
    Vector3d xpnt = new Point3d();
    for (int i = 0; i < num; i++) {
        xpnt.set(pnts[i].x, pnts[i].y, pnts[i].z);
        xpnt.inverseTransform(myX);
        xpnt.updateBounds(min, max);
    }
}

Example 69 with Vector3d

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

the class OBB method render.

public void render(Renderer renderer, int flags) {
    renderer.setShading(Shading.NONE);
    Vector3d hw = myHalfWidths;
    renderer.pushModelMatrix();
    renderer.mulModelMatrix(myX);
    renderer.setColor(0, 0, 1);
    renderer.beginDraw(DrawMode.LINE_STRIP);
    renderer.addVertex(hw.x, hw.y, hw.z);
    renderer.addVertex(-hw.x, hw.y, hw.z);
    renderer.addVertex(-hw.x, -hw.y, hw.z);
    renderer.addVertex(hw.x, -hw.y, hw.z);
    renderer.addVertex(hw.x, hw.y, hw.z);
    renderer.endDraw();
    renderer.beginDraw(DrawMode.LINE_STRIP);
    renderer.addVertex(hw.x, hw.y, -hw.z);
    renderer.addVertex(-hw.x, hw.y, -hw.z);
    renderer.addVertex(-hw.x, -hw.y, -hw.z);
    renderer.addVertex(hw.x, -hw.y, -hw.z);
    renderer.addVertex(hw.x, hw.y, -hw.z);
    renderer.endDraw();
    renderer.setColor(0, 0, 1);
    renderer.beginDraw(DrawMode.LINES);
    renderer.addVertex(hw.x, hw.y, hw.z);
    renderer.addVertex(hw.x, hw.y, -hw.z);
    renderer.addVertex(-hw.x, hw.y, hw.z);
    renderer.addVertex(-hw.x, hw.y, -hw.z);
    renderer.addVertex(-hw.x, -hw.y, hw.z);
    renderer.addVertex(-hw.x, -hw.y, -hw.z);
    renderer.addVertex(hw.x, -hw.y, hw.z);
    renderer.addVertex(hw.x, -hw.y, -hw.z);
    renderer.endDraw();
    renderer.popModelMatrix();
    renderer.setShading(Shading.FLAT);
}

Example 70 with Vector3d

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

the class OBB method intersectsLineSegment.

public boolean intersectsLineSegment(Point3d p1, Point3d p2) {
    Vector3d hw = myHalfWidths;
    Point3d p1t = new Point3d(p1);
    Point3d p2t = new Point3d(p2);
    p1t.inverseTransform(myX);
    p2t.inverseTransform(myX);
    // if either point is inside, return true
    if (Math.abs(p1t.x) <= hw.x && Math.abs(p1t.y) <= hw.y && Math.abs(p1t.z) <= hw.z) {
        return true;
    }
    if (Math.abs(p2t.x) <= hw.x && Math.abs(p2t.y) <= hw.y && Math.abs(p2t.z) <= hw.z) {
        return true;
    }
    // otherwise, intersect with edges
    double[] dir = { p2t.x - p1t.x, p2t.y - p1t.y, p2t.z - p1t.z };
    double[] min = { -hw.x, -hw.y, -hw.z };
    double[] max = { hw.x, hw.y, hw.z };
    double[] p1v = { p1t.x, p1t.y, p1t.z };
    double[] p2v = { p2t.x, p2t.y, p2t.z };
    double near = -INF;
    double far = INF;
    double t1, t2, tMin, tMax;
    // check line/plane intersections
    for (int i = 0; i < 3; i++) {
        if (dir[i] == 0) {
            if ((min[i] - p1v[i] > 0) || (max[i] - p1v[i] < 0)) {
                return false;
            }
        } else {
            t1 = (min[i] - p1v[i]) / dir[i];
            t2 = (min[i] - p2v[i]) / dir[i];
            tMin = Math.min(t1, t2);
            tMax = Math.max(t1, t2);
            if (tMin > near) {
                near = tMin;
            }
            if (tMax < far) {
                far = tMax;
            }
            if (near > far)
                return false;
        }
    }
    if ((near >= 0 && near <= 1) || (far >= 0 && far <= 1)) {
        return true;
    }
    return false;
}