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

Example 1 with Matrix3dBase

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

the class MatricesUBO method putMatrix.

public static void putMatrix(ByteBuffer buff, AffineTransform3dBase T) {
    Matrix3dBase M = T.getMatrix();
    Vector3d b = T.getOffset();
    buff.putFloat((float) M.m00);
    buff.putFloat((float) M.m10);
    buff.putFloat((float) M.m20);
    buff.putFloat((float) 0);
    buff.putFloat((float) M.m01);
    buff.putFloat((float) M.m11);
    buff.putFloat((float) M.m21);
    buff.putFloat((float) 0);
    buff.putFloat((float) M.m02);
    buff.putFloat((float) M.m12);
    buff.putFloat((float) M.m22);
    buff.putFloat((float) 0);
    buff.putFloat((float) b.x);
    buff.putFloat((float) b.y);
    buff.putFloat((float) b.z);
    buff.putFloat((float) 1);
}

Example 2 with Matrix3dBase

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

the class MechSystemSolver method projectSingleFrictionConstraint.

private double projectSingleFrictionConstraint(VectorNd vel, SparseBlockMatrix DT, int bj, double phiMax, double doff, boolean ignoreRigidBodies) {
    int nactive = mySys.numActiveComponents();
    int nparam = mySys.numParametricComponents();
    int nlimit = nactive;
    if (MechSystemBase.myParametricsInSystemMatrix) {
        nlimit += nparam;
    }
    Vector3d d = new Vector3d();
    Vector3d r = new Vector3d();
    double[] vbuf = vel.getBuffer();
    double[] pbuf = myUpar.getBuffer();
    double[] vtbuf = new double[1];
    double phi;
    double dmd = 0;
    for (MatrixBlock blk = DT.firstBlockInCol(bj); blk != null; blk = blk.down()) {
        int bi = blk.getBlockRow();
        if (bi < nactive) {
            blk.mulTransposeAdd(vtbuf, 0, vbuf, DT.getBlockRowOffset(bi));
            if (blk instanceof Matrix3x1) {
                ((Matrix3x1) blk).get(d);
                // XXX assumes that corresponding block is Matrix3d
                Matrix3dBase Minv = (Matrix3dBase) myInverseMass.getBlock(bi, bi);
                Minv.mul(r, d);
                dmd += r.dot(d);
            } else if (!ignoreRigidBodies) {
            // XXX implement
            }
        } else if (MechSystemBase.myParametricsInSystemMatrix && bi < nactive + nparam) {
            int poff = DT.getBlockRowOffset(bi) - myActiveVelSize;
            blk.mulTransposeAdd(vtbuf, 0, pbuf, poff);
        }
    }
    double vt = vtbuf[0] - doff;
    // prevent division by zero
    if (dmd == 0) {
        dmd = 1e-16;
    }
    // System.out.println (" vtMag=" + vt + " dmd=" + dmd + " phiMax="+phiMax);
    if (vt > dmd * phiMax) {
        phi = -phiMax;
    } else if (vt < -dmd * phiMax) {
        phi = phiMax;
    } else {
        phi = -vt / dmd;
    }
    VectorNd dvec = new VectorNd();
    for (MatrixBlock blk = DT.firstBlockInCol(bj); blk != null; blk = blk.down()) {
        int bi = blk.getBlockRow();
        if (bi >= nactive) {
            break;
        }
        if (blk.rowSize() == 3) {
            dvec.setSize(3);
            blk.getColumn(0, dvec);
            MatrixBlock Minv = myInverseMass.getBlock(bi, bi);
            dvec.scale(phi);
            Minv.mulAdd(vbuf, DT.getBlockRowOffset(bi), dvec.getBuffer(), 0);
        } else if (!ignoreRigidBodies) {
        // XXX implement
        }
    }
    return phi;
}

Example 3 with Matrix3dBase

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

the class AffineNumericInputProbe method setTransform.

// fills in a larger transform
public void setTransform(AffineTransform3d A) {
    int nPoints = myVsize / 3;
    Matrix3dBase Ar = A.getMatrix();
    Vector3d At = A.getOffset();
    for (int i = 0; i < nPoints; i++) {
        R.setSubMatrix(i * 3, i * 3, Ar);
        t.setSubVector(i * 3, new VectorNd(At));
    }
}

Example 4 with Matrix3dBase

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

the class MatricesUBO method mul.

private static void mul(Matrix4d P, AffineTransform3dBase M, ByteBuffer buff) {
    Matrix3dBase R = M.getMatrix();
    Vector3d b = M.getOffset();
    buff.putFloat((float) (P.m00 * R.m00 + P.m01 * R.m10 + P.m02 * R.m20));
    buff.putFloat((float) (P.m10 * R.m00 + P.m11 * R.m10 + P.m12 * R.m20));
    buff.putFloat((float) (P.m20 * R.m00 + P.m21 * R.m10 + P.m22 * R.m20));
    buff.putFloat((float) (P.m30 * R.m00 + P.m31 * R.m10 + P.m32 * R.m20));
    buff.putFloat((float) (P.m00 * R.m01 + P.m01 * R.m11 + P.m02 * R.m21));
    buff.putFloat((float) (P.m10 * R.m01 + P.m11 * R.m11 + P.m12 * R.m21));
    buff.putFloat((float) (P.m20 * R.m01 + P.m21 * R.m11 + P.m22 * R.m21));
    buff.putFloat((float) (P.m30 * R.m01 + P.m31 * R.m11 + P.m32 * R.m21));
    buff.putFloat((float) (P.m00 * R.m02 + P.m01 * R.m12 + P.m02 * R.m22));
    buff.putFloat((float) (P.m10 * R.m02 + P.m11 * R.m12 + P.m12 * R.m22));
    buff.putFloat((float) (P.m20 * R.m02 + P.m21 * R.m12 + P.m22 * R.m22));
    buff.putFloat((float) (P.m30 * R.m02 + P.m31 * R.m12 + P.m32 * R.m22));
    buff.putFloat((float) (P.m00 * b.x + P.m01 * b.y + P.m02 * b.z + P.m03));
    buff.putFloat((float) (P.m10 * b.x + P.m11 * b.y + P.m12 * b.z + P.m13));
    buff.putFloat((float) (P.m20 * b.x + P.m21 * b.y + P.m22 * b.z + P.m23));
    buff.putFloat((float) (P.m30 * b.x + P.m31 * b.y + P.m32 * b.z + P.m33));
}

Example 5 with Matrix3dBase

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

the class GLSupport method GLMatrixToTransform.

// public static void GLMatrixToTransform (DenseMatrix T, double[] mat) {
// T.set (0, 0, mat[0]);
// T.set (1, 0, mat[1]);
// T.set (2, 0, mat[2]);
// T.set (3, 0, mat[3]);
// 
// T.set (0, 1, mat[4]);
// T.set (1, 1, mat[5]);
// T.set (2, 1, mat[6]);
// T.set (3, 1, mat[7]);
// 
// T.set (0, 2, mat[8]);
// T.set (1, 2, mat[9]);
// T.set (2, 2, mat[10]);
// T.set (3, 2, mat[11]);
// 
// T.set (0, 3, mat[12]);
// T.set (1, 3, mat[13]);
// T.set (2, 3, mat[14]);
// T.set (3, 3, mat[15]);
// }
// 
// public static void transformToGLMatrix (double[] mat, AffineTransform3d T) {
// mat[0] = T.A.m00;
// mat[1] = T.A.m10;
// mat[2] = T.A.m20;
// mat[3] = 0;
// 
// mat[4] = T.A.m01;
// mat[5] = T.A.m11;
// mat[6] = T.A.m21;
// mat[7] = 0;
// 
// mat[8] = T.A.m02;
// mat[9] = T.A.m12;
// mat[10] = T.A.m22;
// mat[11] = 0;
// 
// mat[12] = T.p.x;
// mat[13] = T.p.y;
// mat[14] = T.p.z;
// mat[15] = 1;
// }
// 
// public static void transformToGLMatrix (double[] mat, int offset, AffineTransform3dBase T) {
// Matrix3dBase M = T.getMatrix();
// Vector3d b = T.getOffset();
// int idx = offset;
// mat[idx++] = M.m00;
// mat[idx++] = M.m10;
// mat[idx++] = M.m20;
// mat[idx++] = 0;
// 
// mat[idx++] = M.m01;
// mat[idx++] = M.m11;
// mat[idx++] = M.m21;
// mat[idx++] = 0;
// 
// mat[idx++] = M.m02;
// mat[idx++] = M.m12;
// mat[idx++] = M.m22;
// mat[idx++] = 0;
// 
// mat[idx++] = b.x;
// mat[idx++] = b.y;
// mat[idx++] = b.z;
// mat[idx++] = 1;
// }
// public static void transformToGLMatrix (float[] mat, int offset, AffineTransform3dBase T) {
// Matrix3dBase M = T.getMatrix();
// Vector3d b = T.getOffset();
// int idx = offset;
// mat[idx++] = (float)M.m00;
// mat[idx++] = (float)M.m10;
// mat[idx++] = (float)M.m20;
// mat[idx++] = 0f;
// 
// mat[idx++] = (float)M.m01;
// mat[idx++] = (float)M.m11;
// mat[idx++] = (float)M.m21;
// mat[idx++] = 0f;
// 
// mat[idx++] = (float)M.m02;
// mat[idx++] = (float)M.m12;
// mat[idx++] = (float)M.m22;
// mat[idx++] = 0f;
// 
// mat[idx++] = (float)b.x;
// mat[idx++] = (float)b.y;
// mat[idx++] = (float)b.z;
// mat[idx++] = 1f;
// }
// 
// public static void transformToGLMatrixTranspose (float[] mat, int offset, AffineTransform3dBase T) {
// Matrix3dBase M = T.getMatrix();
// Vector3d b = T.getOffset();
// int idx = offset;
// mat[idx++] = (float)M.m00;
// mat[idx++] = (float)M.m01;
// mat[idx++] = (float)M.m02;
// mat[idx++] = (float)b.x;
// 
// mat[idx++] = (float)M.m10;
// mat[idx++] = (float)M.m11;
// mat[idx++] = (float)M.m12;
// mat[idx++] = (float)b.y;
// 
// mat[idx++] = (float)M.m20;
// mat[idx++] = (float)M.m21;
// mat[idx++] = (float)M.m22;
// mat[idx++] = (float)b.z;
// 
// mat[idx++] = 0f;
// mat[idx++] = 0f;
// mat[idx++] = 0f;
// mat[idx++] = 1f;
// }
// public static void transformToGLMatrixTranspose (double[] mat, int offset, AffineTransform3dBase T) {
// Matrix3dBase M = T.getMatrix();
// Vector3d b = T.getOffset();
// int idx = offset;
// mat[idx++] = M.m00;
// mat[idx++] = M.m01;
// mat[idx++] = M.m02;
// mat[idx++] = b.x;
// 
// mat[idx++] = M.m10;
// mat[idx++] = M.m11;
// mat[idx++] = M.m12;
// mat[idx++] = b.y;
// 
// mat[idx++] = M.m20;
// mat[idx++] = M.m21;
// mat[idx++] = M.m22;
// mat[idx++] = b.z;
// 
// mat[idx++] = 0.0;
// mat[idx++] = 0.0;
// mat[idx++] = 0.0;
// mat[idx++] = 1.0;
// }
public static void GLMatrixToTransform(AffineTransform3dBase X, double[] mat) {
    Matrix3dBase M = X.getMatrix();
    Vector3d b = X.getOffset();
    M.m00 = mat[0];
    M.m10 = mat[1];
    M.m20 = mat[2];
    M.m01 = mat[4];
    M.m11 = mat[5];
    M.m21 = mat[6];
    M.m02 = mat[8];
    M.m12 = mat[9];
    M.m22 = mat[10];
    b.x = mat[12];
    b.y = mat[13];
    b.z = mat[14];
}