Examples with Matrix3f spacegraph.util.math.Matrix3f used on opensource projects

Example 16 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Point2PointConstraint method buildJacobian.

@Override
public void buildJacobian() {
    appliedImpulse = 0f;
    v3 normal = new v3();
    normal.set(0f, 0f, 0f);
    Matrix3f tmpMat1 = new Matrix3f();
    Matrix3f tmpMat2 = new Matrix3f();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    v3 tmpVec = new v3();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    for (int i = 0; i < 3; i++) {
        VectorUtil.setCoord(normal, i, 1f);
        tmpMat1.transpose(centerOfMassA.basis);
        tmpMat2.transpose(centerOfMassB.basis);
        tmp1.set(pivotInA);
        centerOfMassA.transform(tmp1);
        tmp1.sub(rbA.getCenterOfMassPosition(tmpVec));
        tmp2.set(pivotInB);
        centerOfMassB.transform(tmp2);
        tmp2.sub(rbB.getCenterOfMassPosition(tmpVec));
        jac[i].init(tmpMat1, tmpMat2, tmp1, tmp2, normal, rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        VectorUtil.setCoord(normal, i, 0f);
    }
}

Example 17 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Generic6DofConstraint method calculateLinearInfo.

/**
 * tests linear limits
 */
void calculateLinearInfo() {
    calculatedLinearDiff.sub(calculatedTransformB, calculatedTransformA);
    Matrix3f basisInv = new Matrix3f();
    basisInv.invert(calculatedTransformA.basis);
    // t = this*t      (t is the param)
    basisInv.transform(calculatedLinearDiff);
    linearLimits.currentLinearDiff.set(calculatedLinearDiff);
    for (int i = 0; i < 3; i++) {
        linearLimits.testLimitValue(i, VectorUtil.coord(calculatedLinearDiff, i));
    }
}

Example 18 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Generic6DofConstraint method buildLinearJacobian.

protected void buildLinearJacobian(/*JacobianEntry jacLinear*/
int jacLinear_index, v3 normalWorld, v3 pivotAInW, v3 pivotBInW) {
    Matrix3f mat1 = rbA.getCenterOfMassTransform(new Transform()).basis;
    mat1.transpose();
    Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).basis;
    mat2.transpose();
    v3 tmpVec = new v3();
    v3 tmp1 = new v3();
    tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
    v3 tmp2 = new v3();
    tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
    jacLinear[jacLinear_index].init(mat1, mat2, tmp1, tmp2, normalWorld, rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
}

Example 19 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class Generic6DofConstraint method buildAngularJacobian.

protected void buildAngularJacobian(/*JacobianEntry jacAngular*/
int jacAngular_index, v3 jointAxisW) {
    Matrix3f mat1 = rbA.getCenterOfMassTransform(new Transform()).basis;
    mat1.transpose();
    Matrix3f mat2 = rbB.getCenterOfMassTransform(new Transform()).basis;
    mat2.transpose();
    jacAng[jacAngular_index].init(jointAxisW, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
}

Example 20 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class CapsuleShape method getAabb.

@Override
public void getAabb(Transform t, v3 aabbMin, v3 aabbMax) {
    v3 tmp = new v3();
    v3 halfExtents = new v3();
    halfExtents.set(getRadius(), getRadius(), getRadius());
    VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());
    halfExtents.x += getMargin();
    halfExtents.y += getMargin();
    halfExtents.z += getMargin();
    Matrix3f abs_b = new Matrix3f();
    abs_b.set(t.basis);
    MatrixUtil.absolute(abs_b);
    v3 center = t;
    v3 extent = new v3();
    abs_b.getRow(0, tmp);
    extent.x = tmp.dot(halfExtents);
    abs_b.getRow(1, tmp);
    extent.y = tmp.dot(halfExtents);
    abs_b.getRow(2, tmp);
    extent.z = tmp.dot(halfExtents);
    aabbMin.sub(center, extent);
    aabbMax.add(center, extent);
}