# # old_revision [1b0027b92ff6d7d1eda0b6dbbe9362c303f51b6f] # # patch "src/com/bulletphysics/dynamics/RigidBody.java" # from [48e9a9a61685be5533339bb0ae4af8be6db19bfc] # to [dc5642b4772062bb49890c5bed52a6954ea03105] # # patch "src/com/bulletphysics/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java" # from [659a917da2caba481a243b988d3d373b364fd045] # to [708b161e30736d8427eed9d0b88ad4f7e1970953] # # patch "src/com/bulletphysics/dynamics/constraintsolver/SolverBody.java" # from [8a5b7e3b87b935c53f093eefc19397633edc1a79] # to [2de5c51843b5efbbbcd436b97aae7694b1dab2fb] # # patch "src/com/bulletphysics/linearmath/VectorUtil.java" # from [ecd258a75712014643597880a1e143639daa4c38] # to [c8d256f7ee68454a169d08f79d8f7f5a8fef15cd] # ============================================================ --- src/com/bulletphysics/dynamics/RigidBody.java 48e9a9a61685be5533339bb0ae4af8be6db19bfc +++ src/com/bulletphysics/dynamics/RigidBody.java dc5642b4772062bb49890c5bed52a6954ea03105 @@ -35,6 +35,7 @@ import com.bulletphysics.linearmath.Tran import com.bulletphysics.linearmath.MotionState; import com.bulletphysics.linearmath.Transform; import com.bulletphysics.linearmath.TransformUtil; +import com.bulletphysics.linearmath.VectorUtil; import com.bulletphysics.util.ObjectArrayList; import cz.advel.stack.Stack; import cz.advel.stack.StaticAlloc; @@ -75,7 +76,8 @@ public class RigidBody extends Collision private final Vector3f linearVelocity = new Vector3f(); private final Vector3f angularVelocity = new Vector3f(); private float inverseMass; - private float angularFactor; + private final Vector3f angularFactor = new Vector3f(); + private final Vector3f linearFactor = new Vector3f(); private final Vector3f gravity = new Vector3f(); private final Vector3f invInertiaLocal = new Vector3f(); @@ -125,7 +127,8 @@ public class RigidBody extends Collision linearVelocity.set(0f, 0f, 0f); angularVelocity.set(0f, 0f, 0f); - angularFactor = 1f; + angularFactor.set(1f, 1f, 1f); + linearFactor.set(1f, 1f, 1f); gravity.set(0f, 0f, 0f); totalForce.set(0f, 0f, 0f); totalTorque.set(0f, 0f, 0f); @@ -360,7 +363,9 @@ public class RigidBody extends Collision } public void applyCentralForce(Vector3f force) { - totalForce.add(force); + Vector3f tmp = Stack.alloc(Vector3f.class); + VectorUtil.mul(tmp, force, linearFactor); + totalForce.add(tmp); } public Vector3f getInvInertiaDiagLocal(Vector3f out) { @@ -378,26 +383,31 @@ public class RigidBody extends Collision } public void applyTorque(Vector3f torque) { - totalTorque.add(torque); + Vector3f tmp = Stack.alloc(Vector3f.class); + VectorUtil.mul(tmp, torque, angularFactor); + totalTorque.add(tmp); } public void applyForce(Vector3f force, Vector3f rel_pos) { applyCentralForce(force); Vector3f tmp = Stack.alloc(Vector3f.class); - tmp.cross(rel_pos, force); - tmp.scale(angularFactor); + VectorUtil.mul(tmp, force, linearFactor); + tmp.cross(rel_pos, tmp); applyTorque(tmp); } public void applyCentralImpulse(Vector3f impulse) { - linearVelocity.scaleAdd(inverseMass, impulse, linearVelocity); + Vector3f tmp = Stack.alloc(Vector3f.class); + VectorUtil.mul(tmp, impulse, linearFactor); + linearVelocity.scaleAdd(inverseMass, tmp, linearVelocity); } @StaticAlloc public void applyTorqueImpulse(Vector3f torque) { Vector3f tmp = Stack.alloc(torque); invInertiaTensorWorld.transform(tmp); + VectorUtil.mul(tmp, tmp, angularFactor); angularVelocity.add(tmp); } @@ -405,10 +415,10 @@ public class RigidBody extends Collision public void applyImpulse(Vector3f impulse, Vector3f rel_pos) { if (inverseMass != 0f) { applyCentralImpulse(impulse); - if (angularFactor != 0f) { + if (!VectorUtil.isZero(angularFactor)) { Vector3f tmp = Stack.alloc(Vector3f.class); - tmp.cross(rel_pos, impulse); - tmp.scale(angularFactor); + VectorUtil.mul(tmp, impulse, linearFactor); + tmp.cross(rel_pos, tmp); applyTorqueImpulse(tmp); } } @@ -419,9 +429,12 @@ public class RigidBody extends Collision */ public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) { if (inverseMass != 0f) { - linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity); - if (angularFactor != 0f) { - angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity); + Vector3f tmp = Stack.alloc(Vector3f.class); + VectorUtil.mul(tmp, linearComponent, linearFactor); + linearVelocity.scaleAdd(impulseMagnitude, tmp, linearVelocity); + if (!VectorUtil.isZero(angularFactor)) { + VectorUtil.mul(tmp, angularComponent, angularFactor); + angularVelocity.scaleAdd(impulseMagnitude, tmp, angularVelocity); } } } @@ -572,13 +585,27 @@ public class RigidBody extends Collision } public void setAngularFactor(float angFac) { - angularFactor = angFac; + angularFactor.set(angFac, angFac, angFac); } - public float getAngularFactor() { - return angularFactor; + public void setAngularFactor(Vector3f angFac) { + angularFactor.set(angFac); } + public Vector3f getAngularFactor(Vector3f out) { + out.set(angularFactor); + return out; + } + + public void setLinearFactor(Vector3f linFac) { + linearFactor.set(linFac); + } + + public Vector3f getLinearFactor(Vector3f out) { + out.set(linearFactor); + return out; + } + /** * Is this rigidbody added to a CollisionWorld/DynamicsWorld/Broadphase? */ ============================================================ --- src/com/bulletphysics/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java 659a917da2caba481a243b988d3d373b364fd045 +++ src/com/bulletphysics/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java 708b161e30736d8427eed9d0b88ad4f7e1970953 @@ -36,6 +36,7 @@ import com.bulletphysics.linearmath.Tran import com.bulletphysics.linearmath.MiscUtil; import com.bulletphysics.linearmath.Transform; import com.bulletphysics.linearmath.TransformUtil; +import com.bulletphysics.linearmath.VectorUtil; import com.bulletphysics.util.IntArrayList; import com.bulletphysics.util.ObjectArrayList; import cz.advel.stack.Stack; @@ -148,19 +149,20 @@ public class SequentialImpulseConstraint rb.getAngularVelocity(solverBody.angularVelocity); solverBody.centerOfMassPosition.set(collisionObject.getWorldTransform(Stack.alloc(Transform.class)).origin); solverBody.friction = collisionObject.getFriction(); - solverBody.invMass = rb.getInvMass(); + rb.getLinearFactor(solverBody.invMass); + solverBody.invMass.scale(rb.getInvMass()); rb.getLinearVelocity(solverBody.linearVelocity); solverBody.originalBody = rb; - solverBody.angularFactor = rb.getAngularFactor(); + rb.getAngularFactor(solverBody.angularFactor); } else { solverBody.angularVelocity.set(0f, 0f, 0f); solverBody.centerOfMassPosition.set(collisionObject.getWorldTransform(Stack.alloc(Transform.class)).origin); solverBody.friction = collisionObject.getFriction(); - solverBody.invMass = 0f; + solverBody.invMass.set(0f, 0f, 0f); solverBody.linearVelocity.set(0f, 0f, 0f); solverBody.originalBody = null; - solverBody.angularFactor = 1f; + solverBody.angularFactor.set(1f, 1f, 1f); } solverBody.pushVelocity.set(0f, 0f, 0f); @@ -212,10 +214,10 @@ public class SequentialImpulseConstraint Vector3f tmp = Stack.alloc(Vector3f.class); - tmp.scale(body1.invMass, contactConstraint.contactNormal); + VectorUtil.mul(tmp, contactConstraint.contactNormal, body1.invMass); body1.internalApplyPushImpulse(tmp, contactConstraint.angularComponentA, normalImpulse); - tmp.scale(body2.invMass, contactConstraint.contactNormal); + VectorUtil.mul(tmp, contactConstraint.contactNormal, body2.invMass); body2.internalApplyPushImpulse(tmp, contactConstraint.angularComponentB, -normalImpulse); } } @@ -266,10 +268,10 @@ public class SequentialImpulseConstraint Vector3f tmp = Stack.alloc(Vector3f.class); - tmp.scale(body1.invMass, contactConstraint.contactNormal); + VectorUtil.mul(tmp, contactConstraint.contactNormal, body1.invMass); body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, normalImpulse); - tmp.scale(body2.invMass, contactConstraint.contactNormal); + VectorUtil.mul(tmp, contactConstraint.contactNormal, body2.invMass); body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -normalImpulse); } @@ -329,11 +331,11 @@ public class SequentialImpulseConstraint } Vector3f tmp = Stack.alloc(Vector3f.class); - - tmp.scale(body1.invMass, contactConstraint.contactNormal); + + VectorUtil.mul(tmp, contactConstraint.contactNormal, body1.invMass); body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, j1); - tmp.scale(body2.invMass, contactConstraint.contactNormal); + VectorUtil.mul(tmp, contactConstraint.contactNormal, body2.invMass); body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -j1); } return 0f; @@ -667,11 +669,15 @@ public class SequentialImpulseConstraint if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) { solverConstraint.appliedImpulse = cp.appliedImpulse * infoGlobal.warmstartingFactor; if (rb0 != null) { - tmp.scale(rb0.getInvMass(), solverConstraint.contactNormal); + rb0.getLinearFactor(tmp); + VectorUtil.mul(tmp, tmp, solverConstraint.contactNormal); + tmp.scale(rb0.getInvMass()); tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, solverConstraint.angularComponentA, solverConstraint.appliedImpulse); } if (rb1 != null) { - tmp.scale(rb1.getInvMass(), solverConstraint.contactNormal); + rb1.getLinearFactor(tmp); + VectorUtil.mul(tmp, tmp, solverConstraint.contactNormal); + tmp.scale(rb1.getInvMass()); tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, solverConstraint.angularComponentB, -solverConstraint.appliedImpulse); } } @@ -715,11 +721,15 @@ public class SequentialImpulseConstraint if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) { frictionConstraint1.appliedImpulse = cp.appliedImpulseLateral1 * infoGlobal.warmstartingFactor; if (rb0 != null) { - tmp.scale(rb0.getInvMass(), frictionConstraint1.contactNormal); + rb0.getLinearFactor(tmp); + VectorUtil.mul(tmp, tmp, frictionConstraint1.contactNormal); + tmp.scale(rb0.getInvMass()); tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint1.angularComponentA, frictionConstraint1.appliedImpulse); } if (rb1 != null) { - tmp.scale(rb1.getInvMass(), frictionConstraint1.contactNormal); + rb1.getLinearFactor(tmp); + VectorUtil.mul(tmp, tmp, frictionConstraint1.contactNormal); + tmp.scale(rb1.getInvMass()); tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint1.angularComponentB, -frictionConstraint1.appliedImpulse); } } @@ -732,11 +742,15 @@ public class SequentialImpulseConstraint if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) { frictionConstraint2.appliedImpulse = cp.appliedImpulseLateral2 * infoGlobal.warmstartingFactor; if (rb0 != null) { - tmp.scale(rb0.getInvMass(), frictionConstraint2.contactNormal); + rb0.getLinearFactor(tmp); + VectorUtil.mul(tmp, tmp, frictionConstraint2.contactNormal); + tmp.scale(rb0.getInvMass()); tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint2.angularComponentA, frictionConstraint2.appliedImpulse); } if (rb1 != null) { - tmp.scale(rb1.getInvMass(), frictionConstraint2.contactNormal); + rb1.getLinearFactor(tmp); + VectorUtil.mul(tmp, tmp, frictionConstraint2.contactNormal); + tmp.scale(rb1.getInvMass()); tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint2.angularComponentB, -frictionConstraint2.appliedImpulse); } } ============================================================ --- src/com/bulletphysics/dynamics/constraintsolver/SolverBody.java 8a5b7e3b87b935c53f093eefc19397633edc1a79 +++ src/com/bulletphysics/dynamics/constraintsolver/SolverBody.java 2de5c51843b5efbbbcd436b97aae7694b1dab2fb @@ -26,6 +26,7 @@ import com.bulletphysics.linearmath.Tran import com.bulletphysics.dynamics.RigidBody; import com.bulletphysics.linearmath.Transform; import com.bulletphysics.linearmath.TransformUtil; +import com.bulletphysics.linearmath.VectorUtil; import cz.advel.stack.Stack; import javax.vecmath.Vector3f; @@ -40,8 +41,8 @@ public class SolverBody { //protected final BulletStack stack = BulletStack.get(); public final Vector3f angularVelocity = new Vector3f(); - public float angularFactor; - public float invMass; + public final Vector3f angularFactor = new Vector3f(); + public final Vector3f invMass = new Vector3f(); public float friction; public RigidBody originalBody; public final Vector3f linearVelocity = new Vector3f(); @@ -60,21 +61,31 @@ public class SolverBody { * Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position. */ public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) { - if (invMass != 0f) { + if (!VectorUtil.isZero(invMass)) { linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity); - angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity); + + Vector3f tmp = Stack.alloc(Vector3f.class); + tmp.scale(impulseMagnitude, angularFactor); + VectorUtil.mul(tmp, tmp, angularComponent); + tmp.add(angularVelocity); + angularVelocity.set(tmp); } } public void internalApplyPushImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) { - if (invMass != 0f) { + if (!VectorUtil.isZero(invMass)) { pushVelocity.scaleAdd(impulseMagnitude, linearComponent, pushVelocity); - turnVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, turnVelocity); + + Vector3f tmp = Stack.alloc(Vector3f.class); + tmp.scale(impulseMagnitude, angularFactor); + VectorUtil.mul(tmp, tmp, angularComponent); + tmp.add(turnVelocity); + turnVelocity.set(tmp); } } public void writebackVelocity() { - if (invMass != 0f) { + if (!VectorUtil.isZero(invMass)) { originalBody.setLinearVelocity(linearVelocity); originalBody.setAngularVelocity(angularVelocity); //m_originalBody->setCompanionId(-1); @@ -82,7 +93,7 @@ public class SolverBody { } public void writebackVelocity(float timeStep) { - if (invMass != 0f) { + if (!VectorUtil.isZero(invMass)) { originalBody.setLinearVelocity(linearVelocity); originalBody.setAngularVelocity(angularVelocity); @@ -97,7 +108,7 @@ public class SolverBody { } public void readVelocity() { - if (invMass != 0f) { + if (!VectorUtil.isZero(invMass)) { originalBody.getLinearVelocity(linearVelocity); originalBody.getAngularVelocity(angularVelocity); } ============================================================ --- src/com/bulletphysics/linearmath/VectorUtil.java ecd258a75712014643597880a1e143639daa4c38 +++ src/com/bulletphysics/linearmath/VectorUtil.java c8d256f7ee68454a169d08f79d8f7f5a8fef15cd @@ -190,5 +190,9 @@ public class VectorUtil { dest.x = x; dest.y = y; } + + public static boolean isZero(Vector3f v) { + return v.x == 0f && v.y == 0f && v.z == 0f; + } }