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

Example 1 with Transform

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

the class HingeConstraint method solveConstraint.

@Override
public void solveConstraint(float timeStep) {
    v3 tmp = new v3();
    v3 tmp2 = new v3();
    v3 tmpVec = new v3();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    v3 pivotAInW = new v3(rbAFrame);
    centerOfMassA.transform(pivotAInW);
    v3 pivotBInW = new v3(rbBFrame);
    centerOfMassB.transform(pivotBInW);
    float tau = 0.3f;
    // linear part
    if (!angularOnly) {
        v3 rel_pos1 = new v3();
        rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
        v3 rel_pos2 = new v3();
        rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
        v3 vel1 = rbA.getVelocityInLocalPoint(rel_pos1, new v3());
        v3 vel2 = rbB.getVelocityInLocalPoint(rel_pos2, new v3());
        v3 vel = new v3();
        vel.sub(vel1, vel2);
        for (int i = 0; i < 3; i++) {
            v3 normal = jac[i].linearJointAxis;
            float jacDiagABInv = 1f / jac[i].Adiag;
            float rel_vel;
            rel_vel = normal.dot(vel);
            // positional error (zeroth order error)
            tmp.sub(pivotAInW, pivotBInW);
            // this is the error projected on the normal
            float depth = -(tmp).dot(normal);
            float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
            appliedImpulse += impulse;
            v3 impulse_vector = new v3();
            impulse_vector.scale(impulse, normal);
            tmp.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            rbA.impulse(impulse_vector, tmp);
            tmp.negate(impulse_vector);
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            rbB.impulse(tmp, tmp2);
        }
    }
    // solve angular part
    // get axes in world space
    v3 axisA = new v3();
    rbAFrame.basis.getColumn(2, axisA);
    centerOfMassA.basis.transform(axisA);
    v3 axisB = new v3();
    rbBFrame.basis.getColumn(2, axisB);
    centerOfMassB.basis.transform(axisB);
    v3 angVelA = getRigidBodyA().getAngularVelocity(new v3());
    v3 angVelB = getRigidBodyB().getAngularVelocity(new v3());
    v3 angVelAroundHingeAxisA = new v3();
    angVelAroundHingeAxisA.scale(axisA.dot(angVelA), axisA);
    v3 angVelAroundHingeAxisB = new v3();
    angVelAroundHingeAxisB.scale(axisB.dot(angVelB), axisB);
    v3 angAorthog = new v3();
    angAorthog.sub(angVelA, angVelAroundHingeAxisA);
    v3 angBorthog = new v3();
    angBorthog.sub(angVelB, angVelAroundHingeAxisB);
    v3 velrelOrthog = new v3();
    velrelOrthog.sub(angAorthog, angBorthog);
    // solve orthogonal angular velocity correction
    float relaxation = 1f;
    float len = velrelOrthog.length();
    if (len > 0.00001f) {
        v3 normal = new v3();
        normal.normalize(velrelOrthog);
        float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal);
        // scale for mass and relaxation
        // todo:  expose this 0.9 factor to developer
        velrelOrthog.scale((1f / denom) * relaxationFactor);
    }
    // solve angular positional correction
    // TODO: check
    // v3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
    v3 angularError = new v3();
    angularError.cross(axisA, axisB);
    angularError.negate();
    angularError.scale(1f / timeStep);
    float len2 = angularError.length();
    if (len2 > 0.00001f) {
        v3 normal2 = new v3();
        normal2.normalize(angularError);
        float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2);
        angularError.scale((1f / denom2) * relaxation);
    }
    tmp.negate(velrelOrthog);
    tmp.add(angularError);
    rbA.torqueImpulse(tmp);
    tmp.sub(velrelOrthog, angularError);
    rbB.torqueImpulse(tmp);
    // solve limit
    if (solveLimit) {
        tmp.sub(angVelB, angVelA);
        float amplitude = ((tmp).dot(axisA) * relaxationFactor + correction * (1f / timeStep) * biasFactor) * limitSign;
        float impulseMag = amplitude * kHinge;
        // Clamp the accumulated impulse
        float temp = accLimitImpulse;
        accLimitImpulse = Math.max(accLimitImpulse + impulseMag, 0f);
        impulseMag = accLimitImpulse - temp;
        v3 impulse = new v3();
        impulse.scale(impulseMag * limitSign, axisA);
        rbA.torqueImpulse(impulse);
        tmp.negate(impulse);
        rbB.torqueImpulse(tmp);
    }
    // apply motor
    if (enableAngularMotor) {
        // todo: add limits too
        v3 angularLimit = new v3();
        angularLimit.set(0f, 0f, 0f);
        v3 velrel = new v3();
        velrel.sub(angVelAroundHingeAxisA, angVelAroundHingeAxisB);
        float projRelVel = velrel.dot(axisA);
        float desiredMotorVel = motorTargetVelocity;
        float motor_relvel = desiredMotorVel - projRelVel;
        float unclippedMotorImpulse = kHinge * motor_relvel;
        // todo: should clip against accumulated impulse
        float clippedMotorImpulse = unclippedMotorImpulse > maxMotorImpulse ? maxMotorImpulse : unclippedMotorImpulse;
        clippedMotorImpulse = clippedMotorImpulse < -maxMotorImpulse ? -maxMotorImpulse : clippedMotorImpulse;
        v3 motorImp = new v3();
        motorImp.scale(clippedMotorImpulse, axisA);
        tmp.add(motorImp, angularLimit);
        rbA.torqueImpulse(tmp);
        tmp.negate(motorImp);
        tmp.sub(angularLimit);
        rbB.torqueImpulse(tmp);
    }
}

Example 2 with Transform

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

the class HingeConstraint method buildJacobian.

@Override
public void buildJacobian() {
    v3 tmp = new v3();
    v3 tmp1 = new v3();
    v3 tmp2 = new v3();
    v3 tmpVec = new v3();
    Matrix3f mat1 = new Matrix3f();
    Matrix3f mat2 = new Matrix3f();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    appliedImpulse = 0f;
    if (!angularOnly) {
        v3 pivotAInW = new v3(rbAFrame);
        centerOfMassA.transform(pivotAInW);
        v3 pivotBInW = new v3(rbBFrame);
        centerOfMassB.transform(pivotBInW);
        v3 relPos = new v3();
        relPos.sub(pivotBInW, pivotAInW);
        v3[] normal = /*[3]*/
        { new v3(), new v3(), new v3() };
        if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
            normal[0].set(relPos);
            normal[0].normalize();
        } else {
            normal[0].set(1f, 0f, 0f);
        }
        TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
        for (int i = 0; i < 3; i++) {
            mat1.transpose(centerOfMassA.basis);
            mat2.transpose(centerOfMassB.basis);
            tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            jac[i].init(mat1, mat2, tmp1, tmp2, normal[i], rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
        }
    }
    // calculate two perpendicular jointAxis, orthogonal to hingeAxis
    // these two jointAxis require equal angular velocities for both bodies
    // this is unused for now, it's a todo
    v3 jointAxis0local = new v3();
    v3 jointAxis1local = new v3();
    rbAFrame.basis.getColumn(2, tmp);
    TransformUtil.planeSpace1(tmp, jointAxis0local, jointAxis1local);
    // TODO: check this
    // getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
    v3 jointAxis0 = new v3(jointAxis0local);
    centerOfMassA.basis.transform(jointAxis0);
    v3 jointAxis1 = new v3(jointAxis1local);
    centerOfMassA.basis.transform(jointAxis1);
    v3 hingeAxisWorld = new v3();
    rbAFrame.basis.getColumn(2, hingeAxisWorld);
    centerOfMassA.basis.transform(hingeAxisWorld);
    mat1.transpose(centerOfMassA.basis);
    mat2.transpose(centerOfMassB.basis);
    jacAng[0].init(jointAxis0, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[1].init(jointAxis1, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
    // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
    jacAng[2].init(hingeAxisWorld, mat1, mat2, rbA.getInvInertiaDiagLocal(new v3()), rbB.getInvInertiaDiagLocal(new v3()));
    // Compute limit information
    float hingeAngle = getHingeAngle();
    // set bias, sign, clear accumulator
    correction = 0f;
    limitSign = 0f;
    solveLimit = false;
    accLimitImpulse = 0f;
    if (lowerLimit < upperLimit) {
        if (hingeAngle <= lowerLimit * limitSoftness) {
            correction = (lowerLimit - hingeAngle);
            limitSign = 1.0f;
            solveLimit = true;
        } else if (hingeAngle >= upperLimit * limitSoftness) {
            correction = upperLimit - hingeAngle;
            limitSign = -1.0f;
            solveLimit = true;
        }
    }
    // Compute K = J*W*J' for hinge axis
    v3 axisA = new v3();
    rbAFrame.basis.getColumn(2, axisA);
    centerOfMassA.basis.transform(axisA);
    kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA));
}

Example 3 with Transform

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

the class SliderConstraint method calculateTransforms.

// shared code used by ODE solver
public void calculateTransforms() {
    Transform tmpTrans = new Transform();
    if (useLinearReferenceFrameA) {
        calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
        calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
    } else {
        calculatedTransformA.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
        calculatedTransformB.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
    }
    realPivotAInW.set(calculatedTransformA);
    realPivotBInW.set(calculatedTransformB);
    // along X
    calculatedTransformA.basis.getColumn(0, sliderAxis);
    delta.sub(realPivotBInW, realPivotAInW);
    projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
    v3 normalWorld = new v3();
    // linear part
    for (int i = 0; i < 3; i++) {
        calculatedTransformA.basis.getColumn(i, normalWorld);
        VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
    }
}

Example 4 with Transform

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

the class GImpactCollisionAlgorithm method gimpacttrimeshpart_vs_plane_collision.

protected void gimpacttrimeshpart_vs_plane_collision(Collidable body0, Collidable body1, GImpactMeshShape.GImpactMeshShapePart shape0, StaticPlaneShape shape1, boolean swapped) {
    Transform orgtrans0 = body0.transform;
    Transform orgtrans1 = body1.transform;
    StaticPlaneShape planeshape = shape1;
    Vector4f plane = new Vector4f();
    PlaneShape.get_plane_equation_transformed(planeshape, orgtrans1, plane);
    // test box against plane
    BoxCollision.AABB tribox = new BoxCollision.AABB();
    shape0.getAabb(orgtrans0, tribox.min, tribox.max);
    tribox.increment_margin(planeshape.getMargin());
    if (tribox.plane_classify(plane) != PlaneIntersectionType.COLLIDE_PLANE) {
        return;
    }
    shape0.lockChildShapes();
    float margin = shape0.getMargin() + planeshape.getMargin();
    v3 vertex = new v3();
    v3 tmp = new v3();
    int vi = shape0.getVertexCount();
    float breakingThresh = manifoldPtr.getContactBreakingThreshold();
    while ((vi--) != 0) {
        shape0.getVertex(vi, vertex);
        orgtrans0.transform(vertex);
        float distance = VectorUtil.dot3(vertex, plane) - plane.w - margin;
        if (// add contact
        distance < 0f) {
            if (swapped) {
                tmp.set(-plane.x, -plane.y, -plane.z);
                addContactPoint(body1, body0, vertex, tmp, distance, breakingThresh);
            } else {
                tmp.set(plane.x, plane.y, plane.z);
                addContactPoint(body0, body1, vertex, tmp, distance, breakingThresh);
            }
        }
    }
    shape0.unlockChildShapes();
}

Example 5 with Transform

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

the class GImpactCollisionAlgorithm method gimpact_vs_shape.

public void gimpact_vs_shape(Collidable body0, Collidable body1, GImpactShape shape0, CollisionShape shape1, boolean swapped) {
    ShapeType s = shape0.getGImpactShapeType();
    if (s == ShapeType.TRIMESH_SHAPE) {
        GImpactMeshShape meshshape0 = (GImpactMeshShape) shape0;
        int part0 = meshshape0.getMeshPartCount();
        while ((part0--) != 0) {
            gimpact_vs_shape(body0, body1, meshshape0.getMeshPart(part0), shape1, swapped);
        }
        this.part0 = part0;
        return;
    }
    // #ifdef GIMPACT_VS_PLANE_COLLISION
    if (s == ShapeType.TRIMESH_SHAPE_PART && shape1.getShapeType() == BroadphaseNativeType.STATIC_PLANE_PROXYTYPE) {
        GImpactMeshShape.GImpactMeshShapePart shapepart = (GImpactMeshShape.GImpactMeshShapePart) shape0;
        StaticPlaneShape planeshape = (StaticPlaneShape) shape1;
        gimpacttrimeshpart_vs_plane_collision(body0, body1, shapepart, planeshape, swapped);
        return;
    }
    if (shape1.isCompound()) {
        CompoundShape compoundshape = (CompoundShape) shape1;
        gimpact_vs_compoundshape(body0, body1, shape0, compoundshape, swapped);
        return;
    } else if (shape1.isConcave()) {
        ConcaveShape concaveshape = (ConcaveShape) shape1;
        gimpact_vs_concave(body0, body1, shape0, concaveshape, swapped);
        return;
    }
    Transform orgtrans0 = body0.getWorldTransform(new Transform());
    Transform orgtrans1 = body1.getWorldTransform(new Transform());
    IntArrayList collided_results = new IntArrayList();
    gimpact_vs_shape_find_pairs(orgtrans0, orgtrans1, shape0, shape1, collided_results);
    int cr = collided_results.size();
    if (cr == 0) {
        return;
    }
    shape0.lockChildShapes();
    GIM_ShapeRetriever retriever0 = new GIM_ShapeRetriever(shape0);
    boolean child_has_transform0 = shape0.childrenHasTransform();
    Transform tmpTrans = new Transform();
    int i = cr;
    while ((i--) != 0) {
        int child_index = collided_results.get(i);
        if (swapped) {
            triface1 = child_index;
        } else {
            triface0 = child_index;
        }
        CollisionShape colshape0 = retriever0.getChildShape(child_index);
        if (child_has_transform0) {
            tmpTrans.mul(orgtrans0, shape0.getChildTransform(child_index));
            body0.transform(tmpTrans);
        }
        // collide two shapes
        if (swapped) {
            shape_vs_shape_collision(body1, body0, shape1, colshape0);
        } else {
            shape_vs_shape_collision(body0, body1, colshape0, shape1);
        }
        // restore transforms
        if (child_has_transform0) {
            body0.transform(orgtrans0);
        }
    }
    shape0.unlockChildShapes();
}