Examples with RigidBody com.bulletphysics.dynamics.RigidBody used on opensource projects

Example 6 with RigidBody

use of com.bulletphysics.dynamics.RigidBody in project bdx by GoranM.

the class SequentialImpulseConstraintSolver method prepareConstraints.

protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info, IDebugDraw debugDrawer) {
    Stack stack = Stack.enter();
    RigidBody body0 = (RigidBody) manifoldPtr.getBody0();
    RigidBody body1 = (RigidBody) manifoldPtr.getBody1();
    // only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
    {
        //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
        //manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
        //#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
        int numpoints = manifoldPtr.getNumContacts();
        BulletStats.gTotalContactPoints += numpoints;
        Vector3f tmpVec = stack.allocVector3f();
        Matrix3f tmpMat3 = stack.allocMatrix3f();
        Vector3f pos1 = stack.allocVector3f();
        Vector3f pos2 = stack.allocVector3f();
        Vector3f rel_pos1 = stack.allocVector3f();
        Vector3f rel_pos2 = stack.allocVector3f();
        Vector3f vel1 = stack.allocVector3f();
        Vector3f vel2 = stack.allocVector3f();
        Vector3f vel = stack.allocVector3f();
        Vector3f totalImpulse = stack.allocVector3f();
        Vector3f torqueAxis0 = stack.allocVector3f();
        Vector3f torqueAxis1 = stack.allocVector3f();
        Vector3f ftorqueAxis0 = stack.allocVector3f();
        Vector3f ftorqueAxis1 = stack.allocVector3f();
        for (int i = 0; i < numpoints; i++) {
            ManifoldPoint cp = manifoldPtr.getContactPoint(i);
            if (cp.getDistance() <= 0f) {
                cp.getPositionWorldOnA(pos1);
                cp.getPositionWorldOnB(pos2);
                rel_pos1.sub(pos1, body0.getCenterOfMassPosition(tmpVec));
                rel_pos2.sub(pos2, body1.getCenterOfMassPosition(tmpVec));
                // this jacobian entry is re-used for all iterations
                Matrix3f mat1 = body0.getCenterOfMassTransform(stack.allocTransform()).basis;
                mat1.transpose();
                Matrix3f mat2 = body1.getCenterOfMassTransform(stack.allocTransform()).basis;
                mat2.transpose();
                JacobianEntry jac = jacobiansPool.get();
                jac.init(mat1, mat2, rel_pos1, rel_pos2, cp.normalWorldOnB, body0.getInvInertiaDiagLocal(stack.allocVector3f()), body0.getInvMass(), body1.getInvInertiaDiagLocal(stack.allocVector3f()), body1.getInvMass());
                float jacDiagAB = jac.getDiagonal();
                jacobiansPool.release(jac);
                ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
                if (cpd != null) {
                    // might be invalid
                    cpd.persistentLifeTime++;
                    if (cpd.persistentLifeTime != cp.getLifeTime()) {
                        //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                        //new (cpd) btConstraintPersistentData;
                        cpd.reset();
                        cpd.persistentLifeTime = cp.getLifeTime();
                    } else {
                    //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                    }
                } else {
                    // todo: should this be in a pool?
                    //void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
                    //cpd = new (mem)btConstraintPersistentData;
                    cpd = new ConstraintPersistentData();
                    //assert(cpd != null);
                    totalCpd++;
                    //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                    cp.userPersistentData = cpd;
                    cpd.persistentLifeTime = cp.getLifeTime();
                //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
                }
                assert (cpd != null);
                cpd.jacDiagABInv = 1f / jacDiagAB;
                // Dependent on Rigidbody A and B types, fetch the contact/friction response func
                // perhaps do a similar thing for friction/restutution combiner funcs...
                cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType];
                cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType];
                body0.getVelocityInLocalPoint(rel_pos1, vel1);
                body1.getVelocityInLocalPoint(rel_pos2, vel2);
                vel.sub(vel1, vel2);
                float rel_vel;
                rel_vel = cp.normalWorldOnB.dot(vel);
                float combinedRestitution = cp.combinedRestitution;
                ///btScalar(info.m_numIterations);
                cpd.penetration = cp.getDistance();
                cpd.friction = cp.combinedFriction;
                cpd.restitution = restitutionCurve(rel_vel, combinedRestitution);
                if (cpd.restitution <= 0f) {
                    cpd.restitution = 0f;
                }
                // restitution and penetration work in same direction so
                // rel_vel 
                float penVel = -cpd.penetration / info.timeStep;
                if (cpd.restitution > penVel) {
                    cpd.penetration = 0f;
                }
                float relaxation = info.damping;
                if ((info.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                    cpd.appliedImpulse *= relaxation;
                } else {
                    cpd.appliedImpulse = 0f;
                }
                // for friction
                cpd.prevAppliedImpulse = cpd.appliedImpulse;
                // re-calculate friction direction every frame, todo: check if this is really needed
                TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1);
                //#define NO_FRICTION_WARMSTART 1
                //#ifdef NO_FRICTION_WARMSTART
                cpd.accumulatedTangentImpulse0 = 0f;
                cpd.accumulatedTangentImpulse1 = 0f;
                //#endif //NO_FRICTION_WARMSTART
                float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0);
                float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0);
                float denom = relaxation / (denom0 + denom1);
                cpd.jacDiagABInvTangent0 = denom;
                denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1);
                denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1);
                denom = relaxation / (denom0 + denom1);
                cpd.jacDiagABInvTangent1 = denom;
                //btVector3 totalImpulse = 
                //	//#ifndef NO_FRICTION_WARMSTART
                //	//cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
                //	//cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
                //	//#endif //NO_FRICTION_WARMSTART
                //	cp.normalWorldOnB*cpd.appliedImpulse;
                totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB);
                ///
                {
                    torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
                    cpd.angularComponentA.set(torqueAxis0);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentA);
                    torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
                    cpd.angularComponentB.set(torqueAxis1);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentB);
                }
                {
                    ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
                    cpd.frictionAngularComponent0A.set(ftorqueAxis0);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0A);
                }
                {
                    ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
                    cpd.frictionAngularComponent1A.set(ftorqueAxis1);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1A);
                }
                {
                    ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
                    cpd.frictionAngularComponent0B.set(ftorqueAxis0);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0B);
                }
                {
                    ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
                    cpd.frictionAngularComponent1B.set(ftorqueAxis1);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1B);
                }
                ///
                // apply previous frames impulse on both bodies
                body0.applyImpulse(totalImpulse, rel_pos1);
                tmpVec.negate(totalImpulse);
                body1.applyImpulse(tmpVec, rel_pos2);
            }
        }
    }
    stack.leave();
}

Example 7 with RigidBody

use of com.bulletphysics.dynamics.RigidBody in project bdx by GoranM.

the class SequentialImpulseConstraintSolver method addFrictionConstraint.

@StaticAlloc
protected void addFrictionConstraint(Vector3f normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, Vector3f rel_pos1, Vector3f rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation) {
    RigidBody body0 = RigidBody.upcast(colObj0);
    RigidBody body1 = RigidBody.upcast(colObj1);
    SolverConstraint solverConstraint = constraintsPool.get();
    tmpSolverFrictionConstraintPool.add(solverConstraint);
    solverConstraint.contactNormal.set(normalAxis);
    solverConstraint.solverBodyIdA = solverBodyIdA;
    solverConstraint.solverBodyIdB = solverBodyIdB;
    solverConstraint.constraintType = SolverConstraintType.SOLVER_FRICTION_1D;
    solverConstraint.frictionIndex = frictionIndex;
    solverConstraint.friction = cp.combinedFriction;
    solverConstraint.originalContactPoint = null;
    solverConstraint.appliedImpulse = 0f;
    solverConstraint.appliedPushImpulse = 0f;
    solverConstraint.penetration = 0f;
    Stack stack = Stack.enter();
    Vector3f ftorqueAxis1 = stack.allocVector3f();
    Matrix3f tmpMat = stack.allocMatrix3f();
    {
        ftorqueAxis1.cross(rel_pos1, solverConstraint.contactNormal);
        solverConstraint.relpos1CrossNormal.set(ftorqueAxis1);
        if (body0 != null) {
            solverConstraint.angularComponentA.set(ftorqueAxis1);
            body0.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentA);
        } else {
            solverConstraint.angularComponentA.set(0f, 0f, 0f);
        }
    }
    {
        ftorqueAxis1.cross(rel_pos2, solverConstraint.contactNormal);
        solverConstraint.relpos2CrossNormal.set(ftorqueAxis1);
        if (body1 != null) {
            solverConstraint.angularComponentB.set(ftorqueAxis1);
            body1.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentB);
        } else {
            solverConstraint.angularComponentB.set(0f, 0f, 0f);
        }
    }
    //#ifdef COMPUTE_IMPULSE_DENOM
    //	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
    //	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
    //#else
    Vector3f vec = stack.allocVector3f();
    float denom0 = 0f;
    float denom1 = 0f;
    if (body0 != null) {
        vec.cross(solverConstraint.angularComponentA, rel_pos1);
        denom0 = body0.getInvMass() + normalAxis.dot(vec);
    }
    if (body1 != null) {
        vec.cross(solverConstraint.angularComponentB, rel_pos2);
        denom1 = body1.getInvMass() + normalAxis.dot(vec);
    }
    //#endif //COMPUTE_IMPULSE_DENOM
    float denom = relaxation / (denom0 + denom1);
    solverConstraint.jacDiagABInv = denom;
    stack.leave();
}

Example 8 with RigidBody

use of com.bulletphysics.dynamics.RigidBody 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 9 with RigidBody

use of com.bulletphysics.dynamics.RigidBody in project jmonkeyengine by jMonkeyEngine.

the class PhysicsRigidBody method rebuildRigidBody.

/**
     * Builds/rebuilds the phyiscs body when parameters have changed
     */
protected void rebuildRigidBody() {
    boolean removed = false;
    if (collisionShape instanceof MeshCollisionShape && mass != 0) {
        throw new IllegalStateException("Dynamic rigidbody can not have mesh collision shape!");
    }
    if (rBody != null) {
        if (rBody.isInWorld()) {
            PhysicsSpace.getPhysicsSpace().remove(this);
            removed = true;
        }
        rBody.destroy();
    }
    preRebuild();
    rBody = new RigidBody(constructionInfo);
    postRebuild();
    if (removed) {
        PhysicsSpace.getPhysicsSpace().add(this);
    }
}