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

Example 51 with RotationMatrix3d

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

the class MechSystemSolver method getRBW.

private RotationMatrix3d[] getRBW() {
    RotationMatrix3d[] RBW = new RotationMatrix3d[2 * myNumActive];
    MechSystemBase base = (MechSystemBase) mySys;
    for (int i = 0; i < myNumActive; i++) {
        DynamicComponent c = base.myDynamicComponents.get(i);
        RotationMatrix3d R = new RotationMatrix3d(((RigidBody) c).getPose().R);
        RBW[i * 2 + 0] = R;
        RBW[i * 2 + 1] = R;
    }
    return RBW;
}

Example 52 with RotationMatrix3d

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

the class FrameSpring method addVelJacobianWorld.

public void addVelJacobianWorld(SparseNumberedBlockMatrix M, double s) {
    FrameMaterial mat = myMaterial;
    if (mat == null) {
        // just in case implementation allows null material ...
        return;
    }
    Matrix6dBlock blk00 = getBlock(M, myBlk00Num);
    Matrix6dBlock blk11 = getBlock(M, myBlk11Num);
    Matrix6dBlock blk01 = getBlock(M, myBlk01Num);
    Matrix6dBlock blk10 = getBlock(M, myBlk10Num);
    Matrix6d D = new Matrix6d();
    RotationMatrix3d R1W = new RotationMatrix3d();
    RigidTransform3d XAW = myFrameA.getPose();
    RigidTransform3d XBW;
    if (myFrameB != null) {
        XBW = myFrameB.getPose();
    } else {
        XBW = RigidTransform3d.IDENTITY;
    }
    R1W.mul(XAW.R, myX1A.R);
    Vector3d p1 = new Vector3d();
    Vector3d p2 = new Vector3d();
    p1.transform(XAW.R, myX1A.p);
    p2.transform(XBW.R, myX2B.p);
    // Matrix3d T = myTmpM;
    computeRelativeDisplacements();
    mat.computeDFdu(D, myX21, myVel21, myInitialX21, mySymmetricJacobian);
    D.transform(R1W);
    D.scale(-s);
    if (blk00 != null) {
        myTmp00.set(D);
        postMulMoment(myTmp00, p1);
        preMulMoment(myTmp00, p1);
        blk00.add(myTmp00);
    }
    if (blk11 != null) {
        myTmp11.set(D);
        postMulMoment(myTmp11, p2);
        preMulMoment(myTmp11, p2);
        blk11.add(myTmp11);
    }
    if (blk01 != null && blk10 != null) {
        D.negate();
        myTmp01.set(D);
        postMulMoment(myTmp01, p2);
        preMulMoment(myTmp01, p1);
        myTmp10.set(D);
        postMulMoment(myTmp10, p1);
        preMulMoment(myTmp10, p2);
        blk01.add(myTmp01);
        blk10.add(myTmp10);
    }
}

Example 53 with RotationMatrix3d

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

the class OBB method distanceAlongLine.

/**
 * Code is modified from "An Efficient and Robust Ray-Box Intersection
 * Algorithm", Amy Williams, Steve Barrus, R. Keith Morley, Peter Shirley,
 * University of Utah.
 */
public double distanceAlongLine(Point3d origin, Vector3d dir, double min, double max) {
    // first convert origin and direction to the frame of the OBB
    RotationMatrix3d R = myX.R;
    double x = origin.x - myX.p.x;
    double y = origin.y - myX.p.y;
    double z = origin.z - myX.p.z;
    double ox = R.m00 * x + R.m10 * y + R.m20 * z;
    double oy = R.m01 * x + R.m11 * y + R.m21 * z;
    double oz = R.m02 * x + R.m12 * y + R.m22 * z;
    x = dir.x;
    y = dir.y;
    z = dir.z;
    double dx = R.m00 * x + R.m10 * y + R.m20 * z;
    double dy = R.m01 * x + R.m11 * y + R.m21 * z;
    double dz = R.m02 * x + R.m12 * y + R.m22 * z;
    // now compare transformed point directly to half widths
    Vector3d hw = myHalfWidths;
    double tmin, tmax;
    if (dx == 0) {
        if (-hw.x - ox > 0 || hw.x - ox < 0) {
            return INF;
        }
    } else {
        double divx = 1 / dx;
        if (divx >= 0) {
            tmin = (-hw.x - ox) * divx;
            tmax = (hw.x - ox) * divx;
        } else {
            tmin = (hw.x - ox) * divx;
            tmax = (-hw.x - ox) * divx;
        }
        if (tmin > min) {
            min = tmin;
        }
        if (tmax < max) {
            max = tmax;
        }
        if (min > max) {
            return INF;
        }
    }
    if (dy == 0) {
        if (-hw.y - oy > 0 || hw.y - oy < 0) {
            return INF;
        }
    } else {
        double divy = 1 / dy;
        if (divy >= 0) {
            tmin = (-hw.y - oy) * divy;
            tmax = (hw.y - oy) * divy;
        } else {
            tmin = (hw.y - oy) * divy;
            tmax = (-hw.y - oy) * divy;
        }
        if (tmin > min) {
            min = tmin;
        }
        if (tmax < max) {
            max = tmax;
        }
        if (min > max) {
            return INF;
        }
    }
    if (dz == 0) {
        if (-hw.z - oz > 0 || hw.z - oz < 0) {
            return INF;
        }
    } else {
        double divz = 1 / dz;
        if (divz >= 0) {
            tmin = (-hw.z - oz) * divz;
            tmax = (hw.z - oz) * divz;
        } else {
            tmin = (hw.z - oz) * divz;
            tmax = (-hw.z - oz) * divz;
        }
        if (tmin > min) {
            min = tmin;
        }
        if (tmax < max) {
            max = tmax;
        }
        if (min > max) {
            return INF;
        }
    }
    if (min > 0) {
        return min;
    } else if (max < 0) {
        return -max;
    } else {
        return 0;
    }
}

Example 54 with RotationMatrix3d

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

the class OBB method distanceToPoint.

public double distanceToPoint(Point3d pnt) {
    // first convert point to the frame of the OBB
    RotationMatrix3d R = myX.R;
    double x = pnt.x - myX.p.x;
    double y = pnt.y - myX.p.y;
    double z = pnt.z - myX.p.z;
    double px = R.m00 * x + R.m10 * y + R.m20 * z;
    double py = R.m01 * x + R.m11 * y + R.m21 * z;
    double pz = R.m02 * x + R.m12 * y + R.m22 * z;
    // now compare transformed point directly to half widths
    Vector3d hw = myHalfWidths;
    if ((x = px - hw.x) > 0) {
        if ((y = py - hw.y) > 0) {
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(x * x + y * y + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return Math.sqrt(x * x + y * y);
            } else {
                // pz+hw.z < 0
                return Math.sqrt(x * x + y * y + z * z);
            }
        } else if ((y = py + hw.y) > 0) {
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(x * x + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return x;
            } else {
                // pz+hw.z < 0
                return Math.sqrt(x * x + z * z);
            }
        } else {
            // py+hw.y < 0
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(x * x + y * y + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return Math.sqrt(x * x + y * y);
            } else {
                // pz+hw.z < 0
                return Math.sqrt(x * x + y * y + z * z);
            }
        }
    } else if ((x = px + hw.x) >= 0) {
        if ((y = py - hw.y) > 0) {
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(y * y + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return y;
            } else {
                // pz+hw.z < 0
                return Math.sqrt(y * y + z * z);
            }
        } else if ((y = py + hw.y) > 0) {
            if ((z = pz - hw.z) > 0) {
                return z;
            } else if ((z = pz + hw.z) >= 0) {
                return 0;
            } else {
                // pz+hw.z < 0
                return -z;
            }
        } else {
            // py+hw.y < 0
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(y * y + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return -y;
            } else {
                // pz+hw.z < 0
                return Math.sqrt(y * y + z * z);
            }
        }
    } else {
        // px+hw.x < 0
        if ((y = py - hw.y) > 0) {
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(x * x + y * y + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return Math.sqrt(x * x + y * y);
            } else {
                // pz+hw.z < 0
                return Math.sqrt(x * x + y * y + z * z);
            }
        } else if ((y = py + hw.y) > 0) {
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(x * x + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return -x;
            } else {
                // pz+hw.z < 0
                return Math.sqrt(x * x + z * z);
            }
        } else {
            // py+hw.y < 0
            if ((z = pz - hw.z) > 0) {
                return Math.sqrt(x * x + y * y + z * z);
            } else if ((z = pz + hw.z) >= 0) {
                return Math.sqrt(x * x + y * y);
            } else {
                // pz+hw.z < 0
                return Math.sqrt(x * x + y * y + z * z);
            }
        }
    }
}

Example 55 with RotationMatrix3d

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

the class OBB method updateForPoint.

/**
 * Update the half-widths of this node to accommodate a single point
 * <code>p</code> within a prescribed margin.
 *
 * @param p point to accommodate
 * @param margin extra space margin
 */
public boolean updateForPoint(Point3d p, double margin) {
    // transform x, y, z of p into coordinates bx, by, bz of the coordinate
    // frame of this box.
    boolean modified = false;
    RotationMatrix3d R = myX.R;
    double x = p.x - myX.p.x;
    double y = p.y - myX.p.y;
    double z = p.z - myX.p.z;
    double bx = R.m00 * x + R.m10 * y + R.m20 * z;
    double by = R.m01 * x + R.m11 * y + R.m21 * z;
    double bz = R.m02 * x + R.m12 * y + R.m22 * z;
    double del;
    Vector3d hw = myHalfWidths;
    // now use bx, by, bz to update the bounds
    if ((del = bx + margin - hw.x) > 0) {
        hw.x += del;
        modified = true;
    } else if ((del = -bx + margin - hw.x) > 0) {
        hw.x += del;
        modified = true;
    }
    if ((del = by + margin - hw.y) > 0) {
        hw.y += del;
        modified = true;
    } else if ((del = -by + margin - hw.y) > 0) {
        hw.y += del;
        modified = true;
    }
    if ((del = bz + margin - hw.z) > 0) {
        hw.z += del;
        modified = true;
    } else if ((del = -bz + margin - hw.z) > 0) {
        hw.z += del;
        modified = true;
    }
    return modified;
}