Examples with Transform spacegraph.space3d.phys.math.Transform used on opensource projects

Example 26 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class RetinaPixel method update.

public void update(Dynamics3D d) {
    Transform x = parent.transform();
    worldPosition = x.transform(v(localPosition));
    worldTarget = v(localDirection);
    // TODO limit by contact point
    worldTarget.scale(rangeMax);
    worldTarget.add(localPosition);
    x.transform(worldTarget);
    r = g = b = 0;
    a = distanceToAlpha(rangeMax);
    worldHit.set(worldTarget);
    simplexSolver.reset();
    d.rayTest(worldPosition, worldTarget, this, simplexSolver);
}

Example 27 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class TransformStackList method get.

public Transform get(Transform tr) {
    Transform obj = get();
    obj.set(tr);
    return obj;
}

Example 28 with Transform

use of spacegraph.space3d.phys.math.Transform 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 29 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class HingeConstraint method getHingeAngle.

public float getHingeAngle() {
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    v3 refAxis0 = new v3();
    rbAFrame.basis.getColumn(0, refAxis0);
    centerOfMassA.basis.transform(refAxis0);
    v3 refAxis1 = new v3();
    rbAFrame.basis.getColumn(1, refAxis1);
    centerOfMassA.basis.transform(refAxis1);
    v3 swingAxis = new v3();
    rbBFrame.basis.getColumn(1, swingAxis);
    centerOfMassB.basis.transform(swingAxis);
    return ScalarUtil.atan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
}

Example 30 with Transform

use of spacegraph.space3d.phys.math.Transform 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;
}