Examples with Transform com.bulletphysics.linearmath.Transform used on opensource projects

Example 66 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class SliderConstraint method buildJacobianInt.

// internal
public void buildJacobianInt(RigidBody rbA, RigidBody rbB, Transform frameInA, Transform frameInB) {
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    Transform tmpTrans1 = stack.allocTransform();
    Transform tmpTrans2 = stack.allocTransform();
    Vector3f tmp = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    // calculate transforms
    calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
    calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
    realPivotAInW.set(calculatedTransformA.origin);
    realPivotBInW.set(calculatedTransformB.origin);
    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));
    Vector3f normalWorld = stack.allocVector3f();
    // 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].getDiagonal();
        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();
    Vector3f axisA = stack.allocVector3f();
    calculatedTransformA.basis.getColumn(0, axisA);
    kAngle = 1f / (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
    // clear accumulator for motors
    accumulatedLinMotorImpulse = 0f;
    accumulatedAngMotorImpulse = 0f;
    stack.leave();
}

Example 67 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class SolverBody method writebackVelocity.

public void writebackVelocity(float timeStep) {
    if (invMass != 0f) {
        originalBody.setLinearVelocity(linearVelocity);
        originalBody.setAngularVelocity(angularVelocity);
        Stack stack = Stack.enter();
        // correct the position/orientation based on push/turn recovery
        Transform newTransform = stack.allocTransform();
        Transform curTrans = originalBody.getWorldTransform(stack.allocTransform());
        TransformUtil.integrateTransform(curTrans, pushVelocity, turnVelocity, timeStep, newTransform);
        originalBody.setWorldTransform(newTransform);
        //m_originalBody->setCompanionId(-1);
        stack.leave();
    }
}

Example 68 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class RaycastVehicle method getForwardVector.

/**
	 * Worldspace forward vector.
	 */
public Vector3f getForwardVector(Vector3f out) {
    Stack stack = Stack.enter();
    Transform chassisTrans = getChassisWorldTransform(stack.allocTransform());
    out.set(chassisTrans.basis.getElement(0, indexForwardAxis), chassisTrans.basis.getElement(1, indexForwardAxis), chassisTrans.basis.getElement(2, indexForwardAxis));
    stack.leave();
    return out;
}

Example 69 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class RaycastVehicle method updateFriction.

public void updateFriction(float timeStep) {
    // calculate the impulse, so that the wheels don't move sidewards
    int numWheel = getNumWheels();
    if (numWheel == 0) {
        return;
    }
    MiscUtil.resize(forwardWS, numWheel, Suppliers.NEW_VECTOR3F_SUPPLIER);
    MiscUtil.resize(axle, numWheel, Suppliers.NEW_VECTOR3F_SUPPLIER);
    MiscUtil.resize(forwardImpulse, numWheel, 0f);
    MiscUtil.resize(sideImpulse, numWheel, 0f);
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    int numWheelsOnGround = 0;
    // collapse all those loops into one!
    for (int i = 0; i < getNumWheels(); i++) {
        WheelInfo wheel_info = wheelInfo.getQuick(i);
        RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
        if (groundObject != null) {
            numWheelsOnGround++;
        }
        sideImpulse.set(i, 0f);
        forwardImpulse.set(i, 0f);
    }
    {
        Transform wheelTrans = stack.allocTransform();
        for (int i = 0; i < getNumWheels(); i++) {
            WheelInfo wheel_info = wheelInfo.getQuick(i);
            RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
            if (groundObject != null) {
                getWheelTransformWS(i, wheelTrans);
                Matrix3f wheelBasis0 = stack.alloc(wheelTrans.basis);
                axle.getQuick(i).set(wheelBasis0.getElement(0, indexRightAxis), wheelBasis0.getElement(1, indexRightAxis), wheelBasis0.getElement(2, indexRightAxis));
                Vector3f surfNormalWS = wheel_info.raycastInfo.contactNormalWS;
                float proj = axle.getQuick(i).dot(surfNormalWS);
                tmp.scale(proj, surfNormalWS);
                axle.getQuick(i).sub(tmp);
                axle.getQuick(i).normalize();
                forwardWS.getQuick(i).cross(surfNormalWS, axle.getQuick(i));
                forwardWS.getQuick(i).normalize();
                float[] floatPtr = floatArrays.getFixed(1);
                ContactConstraint.resolveSingleBilateral(chassisBody, wheel_info.raycastInfo.contactPointWS, groundObject, wheel_info.raycastInfo.contactPointWS, 0f, axle.getQuick(i), floatPtr, timeStep);
                sideImpulse.set(i, floatPtr[0]);
                floatArrays.release(floatPtr);
                sideImpulse.set(i, sideImpulse.get(i) * sideFrictionStiffness2);
            }
        }
    }
    float sideFactor = 1f;
    float fwdFactor = 0.5f;
    boolean sliding = false;
    {
        for (int wheel = 0; wheel < getNumWheels(); wheel++) {
            WheelInfo wheel_info = wheelInfo.getQuick(wheel);
            RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
            float rollingFriction = 0f;
            if (groundObject != null) {
                if (wheel_info.engineForce != 0f) {
                    rollingFriction = wheel_info.engineForce * timeStep;
                } else {
                    float defaultRollingFrictionImpulse = 0f;
                    float maxImpulse = wheel_info.brake != 0f ? wheel_info.brake : defaultRollingFrictionImpulse;
                    WheelContactPoint contactPt = new WheelContactPoint(chassisBody, groundObject, wheel_info.raycastInfo.contactPointWS, forwardWS.getQuick(wheel), maxImpulse);
                    rollingFriction = calcRollingFriction(contactPt);
                }
            }
            // switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
            forwardImpulse.set(wheel, 0f);
            wheelInfo.getQuick(wheel).skidInfo = 1f;
            if (groundObject != null) {
                wheelInfo.getQuick(wheel).skidInfo = 1f;
                float maximp = wheel_info.wheelsSuspensionForce * timeStep * wheel_info.frictionSlip;
                float maximpSide = maximp;
                float maximpSquared = maximp * maximpSide;
                //wheelInfo.m_engineForce* timeStep;
                forwardImpulse.set(wheel, rollingFriction);
                float x = (forwardImpulse.get(wheel)) * fwdFactor;
                float y = (sideImpulse.get(wheel)) * sideFactor;
                float impulseSquared = (x * x + y * y);
                if (impulseSquared > maximpSquared) {
                    sliding = true;
                    float factor = maximp / (float) Math.sqrt(impulseSquared);
                    wheelInfo.getQuick(wheel).skidInfo *= factor;
                }
            }
        }
    }
    if (sliding) {
        for (int wheel = 0; wheel < getNumWheels(); wheel++) {
            if (sideImpulse.get(wheel) != 0f) {
                if (wheelInfo.getQuick(wheel).skidInfo < 1f) {
                    forwardImpulse.set(wheel, forwardImpulse.get(wheel) * wheelInfo.getQuick(wheel).skidInfo);
                    sideImpulse.set(wheel, sideImpulse.get(wheel) * wheelInfo.getQuick(wheel).skidInfo);
                }
            }
        }
    }
    // apply the impulses
    {
        for (int wheel = 0; wheel < getNumWheels(); wheel++) {
            WheelInfo wheel_info = wheelInfo.getQuick(wheel);
            Vector3f rel_pos = stack.allocVector3f();
            rel_pos.sub(wheel_info.raycastInfo.contactPointWS, chassisBody.getCenterOfMassPosition(tmp));
            if (forwardImpulse.get(wheel) != 0f) {
                tmp.scale(forwardImpulse.get(wheel), forwardWS.getQuick(wheel));
                chassisBody.applyImpulse(tmp, rel_pos);
            }
            if (sideImpulse.get(wheel) != 0f) {
                RigidBody groundObject = (RigidBody) wheelInfo.getQuick(wheel).raycastInfo.groundObject;
                Vector3f rel_pos2 = stack.allocVector3f();
                rel_pos2.sub(wheel_info.raycastInfo.contactPointWS, groundObject.getCenterOfMassPosition(tmp));
                Vector3f sideImp = stack.allocVector3f();
                sideImp.scale(sideImpulse.get(wheel), axle.getQuick(wheel));
                rel_pos.z *= wheel_info.rollInfluence;
                chassisBody.applyImpulse(sideImp, rel_pos);
                // apply friction impulse on the ground
                tmp.negate(sideImp);
                groundObject.applyImpulse(tmp, rel_pos2);
            }
        }
    }
    stack.leave();
}

Example 70 with Transform

use of com.bulletphysics.linearmath.Transform in project bdx by GoranM.

the class GImpactCollisionAlgorithm method gimpact_vs_gimpact.

public void gimpact_vs_gimpact(CollisionObject body0, CollisionObject body1, GImpactShapeInterface shape0, GImpactShapeInterface shape1) {
    if (shape0.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE) {
        GImpactMeshShape meshshape0 = (GImpactMeshShape) shape0;
        part0 = meshshape0.getMeshPartCount();
        while ((part0--) != 0) {
            gimpact_vs_gimpact(body0, body1, meshshape0.getMeshPart(part0), shape1);
        }
        return;
    }
    if (shape1.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE) {
        GImpactMeshShape meshshape1 = (GImpactMeshShape) shape1;
        part1 = meshshape1.getMeshPartCount();
        while ((part1--) != 0) {
            gimpact_vs_gimpact(body0, body1, shape0, meshshape1.getMeshPart(part1));
        }
        return;
    }
    Stack stack = Stack.enter();
    Transform orgtrans0 = body0.getWorldTransform(stack.allocTransform());
    Transform orgtrans1 = body1.getWorldTransform(stack.allocTransform());
    PairSet pairset = tmpPairset;
    pairset.clear();
    gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset);
    if (pairset.size() == 0) {
        stack.leave();
        return;
    }
    if (shape0.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE_PART && shape1.getGImpactShapeType() == ShapeType.TRIMESH_SHAPE_PART) {
        GImpactMeshShapePart shapepart0 = (GImpactMeshShapePart) shape0;
        GImpactMeshShapePart shapepart1 = (GImpactMeshShapePart) shape1;
        //specialized function
        //#ifdef BULLET_TRIANGLE_COLLISION
        //collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
        //#else
        collide_sat_triangles(body0, body1, shapepart0, shapepart1, pairset, pairset.size());
        //#endif
        stack.leave();
        return;
    }
    // general function
    shape0.lockChildShapes();
    shape1.lockChildShapes();
    GIM_ShapeRetriever retriever0 = new GIM_ShapeRetriever(shape0);
    GIM_ShapeRetriever retriever1 = new GIM_ShapeRetriever(shape1);
    boolean child_has_transform0 = shape0.childrenHasTransform();
    boolean child_has_transform1 = shape1.childrenHasTransform();
    Transform tmpTrans = stack.allocTransform();
    int i = pairset.size();
    while ((i--) != 0) {
        Pair pair = pairset.get(i);
        triface0 = pair.index1;
        triface1 = pair.index2;
        CollisionShape colshape0 = retriever0.getChildShape(triface0);
        CollisionShape colshape1 = retriever1.getChildShape(triface1);
        if (child_has_transform0) {
            tmpTrans.mul(orgtrans0, shape0.getChildTransform(triface0));
            body0.setWorldTransform(tmpTrans);
        }
        if (child_has_transform1) {
            tmpTrans.mul(orgtrans1, shape1.getChildTransform(triface1));
            body1.setWorldTransform(tmpTrans);
        }
        // collide two convex shapes
        convex_vs_convex_collision(body0, body1, colshape0, colshape1);
        if (child_has_transform0) {
            body0.setWorldTransform(orgtrans0);
        }
        if (child_has_transform1) {
            body1.setWorldTransform(orgtrans1);
        }
    }
    shape0.unlockChildShapes();
    shape1.unlockChildShapes();
    stack.leave();
}