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

Example 11 with Matrix3f

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

the class TransformUtil method calculateDiffAxisAngle.

public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, v3 axis, float[] angle) {
    // #ifdef USE_QUATERNION_DIFF
    // btQuaternion orn0 = transform0.getRotation();
    // btQuaternion orn1a = transform1.getRotation();
    // btQuaternion orn1 = orn0.farthest(orn1a);
    // btQuaternion dorn = orn1 * orn0.inverse();
    // #else
    Matrix3f tmp = new Matrix3f();
    tmp.set(transform0.basis);
    MatrixUtil.invert(tmp);
    Matrix3f dmat = new Matrix3f();
    dmat.mul(transform1.basis, tmp);
    Quat4f dorn = new Quat4f();
    MatrixUtil.getRotation(dmat, dorn);
    // #endif
    // floating point inaccuracy can lead to w component > 1..., which breaks
    dorn.normalize();
    angle[0] = QuaternionUtil.getAngle(dorn);
    axis.set(dorn.x, dorn.y, dorn.z);
    // TODO: probably not needed
    // axis[3] = btScalar(0.);
    // check for axis length
    float lenSq = axis.lengthSquared();
    if (lenSq < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
        axis.set(1f, 0f, 0f);
    } else {
        axis.scale(1f / (float) Math.sqrt(lenSq));
    }
}

Example 12 with Matrix3f

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

the class CompoundShape method getAabb.

/**
 * getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version.
 */
@Override
public void getAabb(Transform trans, v3 aabbMin, v3 aabbMax) {
    v3 localHalfExtents = new v3();
    localHalfExtents.sub(localAabbMax, localAabbMin);
    localHalfExtents.scale(0.5f);
    localHalfExtents.x += collisionMargin;
    localHalfExtents.y += collisionMargin;
    localHalfExtents.z += collisionMargin;
    v3 localCenter = new v3();
    localCenter.add(localAabbMax, localAabbMin);
    localCenter.scale(0.5f);
    Matrix3f abs_b = new Matrix3f(trans.basis);
    MatrixUtil.absolute(abs_b);
    v3 center = new v3(localCenter);
    trans.transform(center);
    v3 tmp = new v3();
    v3 extent = new v3();
    abs_b.getRow(0, tmp);
    extent.x = tmp.dot(localHalfExtents);
    abs_b.getRow(1, tmp);
    extent.y = tmp.dot(localHalfExtents);
    abs_b.getRow(2, tmp);
    extent.z = tmp.dot(localHalfExtents);
    aabbMin.sub(center, extent);
    aabbMax.add(center, extent);
}

Example 13 with Matrix3f

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

the class MatrixStackList method get.

public Matrix3f get(Matrix3f mat) {
    Matrix3f obj = get();
    obj.set(mat);
    return obj;
}

Example 14 with Matrix3f

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

the class ContactConstraint method resolveSingleBilateral.

/**
 * Bilateral constraint between two dynamic objects.
 */
public static void resolveSingleBilateral(Body3D body1, v3 pos1, Body3D body2, v3 pos2, float distance, v3 normal, float[] impulse, float timeStep) {
    float normalLenSqr = normal.lengthSquared();
    assert (Math.abs(normalLenSqr) < 1.1f);
    if (normalLenSqr > 1.1f) {
        impulse[0] = 0f;
        return;
    }
    v3 tmp = new v3();
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmp));
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmp));
    // this jacobian entry could be re-used for all iterations
    v3 vel1 = new v3();
    body1.getVelocityInLocalPoint(rel_pos1, vel1);
    v3 vel2 = new v3();
    body2.getVelocityInLocalPoint(rel_pos2, vel2);
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    Matrix3f mat1 = body1.getCenterOfMassTransform(new Transform()).basis;
    mat1.transpose();
    Matrix3f mat2 = body2.getCenterOfMassTransform(new Transform()).basis;
    mat2.transpose();
    JacobianEntry jac = new JacobianEntry();
    jac.init(mat1, mat2, rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(new v3()), body1.getInvMass(), body2.getInvInertiaDiagLocal(new v3()), body2.getInvMass());
    float jacDiagAB = jac.Adiag;
    float jacDiagABInv = 1f / jacDiagAB;
    v3 tmp1 = body1.getAngularVelocity(new v3());
    mat1.transform(tmp1);
    v3 tmp2 = body2.getAngularVelocity(new v3());
    mat2.transform(tmp2);
    float rel_vel = jac.getRelativeVelocity(body1.getLinearVelocity(new v3()), tmp1, body2.getLinearVelocity(new v3()), tmp2);
    float a;
    a = jacDiagABInv;
    rel_vel = normal.dot(vel);
    // todo: move this into proper structure
    float contactDamping = 0.2f;
    // #ifdef ONLY_USE_LINEAR_MASS
    // btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
    // impulse = - contactDamping * rel_vel * massTerm;
    // #else
    float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
    impulse[0] = velocityImpulse;
// #endif
}

Example 15 with Matrix3f

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

the class SliderConstraint method buildJacobianInt.

// internal
public void buildJacobianInt(Body3D rbA, Body3D rbB, Transform frameInA, Transform frameInB) {
    Transform tmpTrans = new Transform();
    Transform tmpTrans1 = new Transform();
    Transform tmpTrans2 = new Transform();
    v3 tmp = new v3();
    v3 tmp2 = new v3();
    // calculate transforms
    calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
    calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
    realPivotAInW.set(calculatedTransformA);
    realPivotBInW.set(calculatedTransformB);
    calculatedTransformA.basis.getColumn(0, tmp);
    // along X
    sliderAxis.set(tmp);
    delta.sub(realPivotBInW, realPivotAInW);
    projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
    relPosA.sub(projPivotInW, rbA.getCenterOfMassPosition(tmp));
    relPosB.sub(realPivotBInW, rbB.getCenterOfMassPosition(tmp));
    v3 normalWorld = new v3();
    // linear part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        Matrix3f mat1 = rbA.getCenterOfMassTransform(tmpTrans1).basis;
        mat1.transpose();
        Matrix3f mat2 = rbB.getCenterOfMassTransform(tmpTrans2).basis;
        mat2.transpose();
        jacLin[i].init(mat1, mat2, relPosA, relPosB, normalWorld, rbA.getInvInertiaDiagLocal(tmp), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(tmp2), rbB.getInvMass());
        jacLinDiagABInv[i] = 1f / jacLin[i].Adiag;
        VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
    }
    testLinLimits();
    // angular part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        Matrix3f mat1 = rbA.getCenterOfMassTransform(tmpTrans1).basis;
        mat1.transpose();
        Matrix3f mat2 = rbB.getCenterOfMassTransform(tmpTrans2).basis;
        mat2.transpose();
        jacAng[i].init(normalWorld, mat1, mat2, rbA.getInvInertiaDiagLocal(tmp), rbB.getInvInertiaDiagLocal(tmp2));
    }
    testAngLimits();
    v3 axisA = new v3();
    calculatedTransformA.basis.getColumn(0, axisA);
    kAngle = 1f / (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
    // clear accumulator for motors
    accumulatedLinMotorImpulse = 0f;
    accumulatedAngMotorImpulse = 0f;
}