public virtual void drawArc( ref btVector3 center, ref btVector3 normal, ref btVector3 axis , double radiusA, double radiusB, double minAngle, double maxAngle, ref btVector3 color, bool drawSect, double stepDegrees = btScalar.BT_TEN ) { btVector3 vx = axis; btVector3 vy; normal.cross( ref axis, out vy ); double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG; int nSteps = (int)btScalar.btFabs( ( maxAngle - minAngle ) / step ); if( nSteps == 0 ) nSteps = 1; btVector3 prev; center.AddScale( ref vx, radiusA * btScalar.btCos( minAngle ), out prev ); prev.AddScale( ref vy, radiusB * btScalar.btSin( minAngle ), out prev ); if( drawSect ) { drawLine( ref center, ref prev, ref color ); } for( int i = 1; i <= nSteps; i++ ) { double angle = minAngle + ( maxAngle - minAngle ) * i / (double)nSteps; btVector3 next; center.AddScale( ref vx, radiusA * btScalar.btCos( angle ), out next ); next.AddScale( ref vy, radiusB * btScalar.btSin( angle ), out next ); drawLine( ref prev, ref next, ref color ); prev = next; } if( drawSect ) { drawLine( ref center, ref prev, ref color ); } }
public static void shortestArcQuat( ref btVector3 v0, ref btVector3 v1, out btQuaternion result ) // Game Programming Gems 2.10 make sure v0,v1 are normalized { btVector3 c; v0.cross( ref v1, out c ); double d = v0.dot( ref v1 ); if( d < -1.0 + btScalar.SIMD_EPSILON ) { btVector3 n, unused; btVector3.btPlaneSpace1( ref v0, out n, out unused ); result.x = n.x; result.y = n.y; result.z = n.z; result.w = 0.0f; // just pick any vector that is orthogonal to v0 return; } double s = btScalar.btSqrt( ( 1.0f + d ) * 2.0f ); double rs = 1.0f / s; result.x = c.x * rs; result.y = c.y * rs; result.z = c.z * rs; result.w = s * 0.5f; }
public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt, ref btMatrix3x3 I, out btVector3 result ) { btVector3 Iw0; btVector3 dtT; btVector3 Iw1; btVector3 cross; T.Mult( dt, out dtT ); I.Apply( ref w0, out Iw0 ); dtT.Add( ref Iw0, out Iw0 ); // Iw0 = ( T * dt + I * w0 ) I.Apply( ref w1, out Iw1 ); w1.cross( ref Iw1, out cross ); cross.Mult( dt, out cross ); // cross = w1.cross( I * w1 ) * dt Iw1.Add( ref cross, out Iw1 ); // Iw1 = Iw1 + cross Iw1.Sub( ref Iw0, out result ); //btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 ); //return w2; }
/// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. ///Solve33 is from Box2d, thanks to Erin Catto, public void solve33( ref btVector3 b, out btVector3 result ) { btVector3 col1 = getColumn( 0 ); btVector3 col2 = getColumn( 1 ); btVector3 col3 = getColumn( 2 ); btVector3 tmp; col2.cross( ref col3, out tmp ); double det = col1.dot( ref tmp ); if( btScalar.btFabs( det ) > btScalar.SIMD_EPSILON ) { det = 1.0f / det; } col2.cross( ref col3, out tmp ); result.x = det * b.dot( ref tmp ); b.cross( ref col3, out tmp ); result.y = det * col1.dot( ref tmp ); col2.cross( ref b, out tmp ); result.z = det * col1.dot( ref tmp ); result.w = 0; }
public btHingeConstraint( btRigidBody rbA, btRigidBody rbB, ref btVector3 pivotInA, ref btVector3 pivotInB, ref btVector3 axisInA, ref btVector3 axisInB, bool useReferenceFrameA = false ) : base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA, rbB ) { Init(); #if _BT_USE_CENTER_LIMIT_ m_limit = new btAngularLimit(); #endif m_useReferenceFrameA = ( useReferenceFrameA ); m_rbAFrame.m_origin = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1; rbA.m_worldTransform.m_basis.getColumn( 0, out rbAxisA1 ); btVector3 rbAxisA2; double projection = axisInA.dot( rbAxisA1 ); if( projection >= 1.0f - btScalar.SIMD_EPSILON ) { btVector3 tmp; rbA.m_worldTransform.m_basis.getColumn( 2, out tmp ); tmp.Invert( out rbAxisA1 ); rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 ); } else if( projection <= -1.0f + btScalar.SIMD_EPSILON ) { rbA.m_worldTransform.m_basis.getColumn( 2, out rbAxisA1 ); rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 ); } else { axisInA.cross( ref rbAxisA1, out rbAxisA2 ); rbAxisA2.cross( ref axisInA, out rbAxisA1 ); } btMatrix3x3.setValue( out m_rbAFrame.m_basis, rbAxisA1.x, rbAxisA2.x, axisInA.x, rbAxisA1.y, rbAxisA2.y, axisInA.y, rbAxisA1.z, rbAxisA2.z, axisInA.z ); btQuaternion rotationArc; btQuaternion.shortestArcQuat( ref axisInA, ref axisInB, out rotationArc ); btVector3 rbAxisB1; btQuaternion.quatRotate( ref rotationArc, ref rbAxisA1, out rbAxisB1 ); btVector3 rbAxisB2; axisInB.cross( ref rbAxisB1, out rbAxisB2 ); m_rbBFrame.m_origin = pivotInB; m_rbBFrame.m_basis.setValue( ref rbAxisB1, ref rbAxisB2, ref axisInB ); btMatrix3x3.setValue( out m_rbBFrame.m_basis, ref rbAxisB1, ref rbAxisB2, ref axisInB ); m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 ); }
internal void setupContactConstraint( btSolverConstraint solverConstraint, btSolverBody bodyA, btSolverBody bodyB, btManifoldPoint cp, btContactSolverInfo infoGlobal, out double relaxation, ref btVector3 rel_pos1, ref btVector3 rel_pos2 ) { // ref btVector3 pos1 = cp.getPositionWorldOnA(); // ref btVector3 pos2 = cp.getPositionWorldOnB(); btRigidBody rb0 = bodyA.m_originalBody; btRigidBody rb1 = bodyB.m_originalBody; // btVector3 rel_pos1 = pos1 - colObj0.getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1.getWorldTransform().getOrigin(); //rel_pos1 = pos1 - bodyA.getWorldTransform().getOrigin(); //rel_pos2 = pos2 - bodyB.getWorldTransform().getOrigin(); relaxation = 1; btVector3 torqueAxis0; rel_pos1.cross( ref cp.m_normalWorldOnB, out torqueAxis0 ); btVector3 tmp; //solverConstraint.m_angularComponentA = rb0 != null ? rb0.m_invInertiaTensorWorld * torqueAxis0 * rb0.getAngularFactor() : btVector3.Zero; if( rb0 != null ) { rb0.m_invInertiaTensorWorld.Apply( ref torqueAxis0, out tmp ); tmp.Mult( ref rb0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else solverConstraint.m_angularComponentA = btVector3.Zero; btVector3 torqueAxis1; rel_pos2.cross( ref cp.m_normalWorldOnB, out torqueAxis1 ); torqueAxis1.Invert( out torqueAxis1 ); //solverConstraint.m_angularComponentB = rb1 != null ? rb1.m_invInertiaTensorWorld * -torqueAxis1 * rb1.getAngularFactor() : btVector3.Zero; if( rb1 != null ) { rb1.m_invInertiaTensorWorld.Apply( ref torqueAxis1, out tmp ); tmp.Mult( ref rb1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else solverConstraint.m_angularComponentB = btVector3.Zero; { #if COMPUTE_IMPULSE_DENOM double denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB); double denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else btVector3 vec; double denom0 = 0; double denom1 = 0; if( rb0 != null ) { ( solverConstraint.m_angularComponentA ).cross( ref rel_pos1, out vec ); denom0 = rb0.getInvMass() + cp.m_normalWorldOnB.dot( vec ); } if( rb1 != null ) { solverConstraint.m_angularComponentB.Invert( out tmp ); tmp.cross( ref rel_pos2, out vec ); denom1 = rb1.getInvMass() + cp.m_normalWorldOnB.dot( vec ); } #endif //COMPUTE_IMPULSE_DENOM double denom = relaxation / ( denom0 + denom1 ); btScalar.Dbg( "m_jacDiagABInv 3 set to " + denom.ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = denom; } if( rb0 != null ) { solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; solverConstraint.m_relpos1CrossNormal = torqueAxis0; btScalar.Dbg( "Torque Axis to relpos1 " + torqueAxis0 ); } else { solverConstraint.m_contactNormal1 = btVector3.Zero; solverConstraint.m_relpos1CrossNormal = btVector3.Zero; } if( rb1 != null ) { cp.m_normalWorldOnB.Invert( out solverConstraint.m_contactNormal2 ); solverConstraint.m_relpos2CrossNormal = torqueAxis1; btScalar.Dbg( "Torque Axis to relpos2 " + torqueAxis1 ); } else { solverConstraint.m_contactNormal2 = btVector3.Zero; solverConstraint.m_relpos2CrossNormal = btVector3.Zero; } double restitution = 0; double penetration = cp.getDistance() + infoGlobal.m_linearSlop; { btVector3 vel1, vel2; vel1 = rb0 != null ? rb0.getVelocityInLocalPoint( ref rel_pos1 ) : btVector3.Zero; vel2 = rb1 != null ? rb1.getVelocityInLocalPoint( ref rel_pos2 ) : btVector3.Zero; // btVector3 vel2 = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); btVector3 vel; vel1.Sub( ref vel2, out vel ); double rel_vel = cp.m_normalWorldOnB.dot( ref vel ); solverConstraint.m_friction = cp.m_combinedFriction; restitution = restitutionCurve( rel_vel, cp.m_combinedRestitution ); if( restitution <= btScalar.BT_ZERO ) { restitution = 0; }; } ///warm starting (or zero if disabled) if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 ) { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if( rb0 != null ) { solverConstraint.m_contactNormal1.Mult2( ref bodyA.m_invMass, ref rb0.m_linearFactor, out tmp ); bodyA.applyImpulse( ref tmp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse ); } if( rb1 != null ) { solverConstraint.m_contactNormal2.Mult2( ref rb1.m_linearFactor, ref bodyB.m_invMass, out tmp ); tmp.Invert( out tmp ); btVector3 tmp2; solverConstraint.m_angularComponentB.Invert( out tmp2 ); bodyB.applyImpulse( ref tmp, ref tmp2, -(double)solverConstraint.m_appliedImpulse ); } } else { solverConstraint.m_appliedImpulse = 0; } solverConstraint.m_appliedPushImpulse = 0; { btVector3 externalForceImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalTorqueImpulse : btVector3.Zero; btVector3 externalForceImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalForceImpulse : btVector3.Zero; btVector3 externalTorqueImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalTorqueImpulse : btVector3.Zero; btScalar.Dbg( "external torque impulses " + externalTorqueImpulseA + externalTorqueImpulseB ); double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref bodyA.m_linearVelocity, ref externalForceImpulseA ) + solverConstraint.m_relpos1CrossNormal.dotAdded( ref bodyA.m_angularVelocity, ref externalTorqueImpulseA ); double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref bodyB.m_linearVelocity, ref externalForceImpulseB ) + solverConstraint.m_relpos2CrossNormal.dotAdded( ref bodyB.m_angularVelocity, ref externalTorqueImpulseB ); double rel_vel = vel1Dotn + vel2Dotn; double positionalError = 0; double velocityError = restitution - rel_vel;// * damping; double erp = infoGlobal.m_erp2; if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) ) { erp = infoGlobal.m_erp; } if( penetration > 0 ) { positionalError = 0; velocityError -= penetration / infoGlobal.m_timeStep; } else { positionalError = -penetration * erp / infoGlobal.m_timeStep; } double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) ) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA.m_externalForce*bodyA.m_invMass-bodyB.m_externalForce/bodyB.m_invMass)*solverConstraint.m_jacDiagABInv; btScalar.Dbg( "Constraint 3 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = 0; } else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 4 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = penetrationImpulse; } solverConstraint.m_cfm = 0; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } }
static void Main(string[] args) { btVector3 testvec = new btVector3(-2, 1, 0); Console.WriteLine(String.Format("Original: {0}", testvec.testStr())); btVector3 testvec2 = testvec.absolute(); Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr())); Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2))); Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis())); btVector3 testvec3 = testvec.cross(testvec2); Console.WriteLine(String.Format("cross: {0}", testvec3.testStr())); Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2))); Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2))); Console.WriteLine(String.Format("dot: {0}", testvec.dot(testvec2))); Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis())); btVector3 testvec4 = testvec.normalized(); Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr())); testvec4.setInterpolate3(testvec, testvec2, 0.5f); Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr())); testvec4.setValue(7f, -0.09f, 2.5f); Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr())); testvec4.setX(5.0f); testvec4.setY(-0.25f); testvec4.setZ(90f); testvec.setValue(0, 0, -1024); testvec2.setValue(256, 256, 1024); Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr())); btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50); btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration(); btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig); btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver(); btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol, colconfig); dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f)); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); btQuaternion testquat = new btQuaternion(50, 0, 0, 1); btQuaternion testquatnorm = testquat.normalized(); Console.WriteLine(String.Format("testquat: {0}", testquat.testStr())); Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr())); Console.WriteLine(String.Format("testquatLength: {0}", testquat.length())); Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length())); float[] heightdata = new float[256 * 256]; for (int j = 0; j < 256 * 256; j++) { if (j % 2 == 0) { heightdata[j] = 21f; } else { heightdata[j] = 28f; } } btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256, (int)btHeightfieldTerrainShape.UPAxis.Z, (int)btHeightfieldTerrainShape.PHY_ScalarType. PHY_FLOAT, false); btCapsuleShape cap = new btCapsuleShape(0.23f, 3); btTriangleMesh testMesh = new btTriangleMesh(true, false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false); btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh); meshtest.updateBound(); btRigidBody groundbody = new btRigidBody(0, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 256f / 2f))), obj, new btVector3(0, 0, 0)); btRigidBody capbody = new btRigidBody(200, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 25))), cap, new btVector3(0, 0, 0)); btRigidBody meshbody = new btRigidBody(200, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 29))), meshtest, new btVector3(0, 0, 0)); btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo(); constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f, 0.5f, 0.5f)); constructioninfotest.m_localInertia = new btVector3(0, 0, 0); constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)), new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0.25f, 0))); constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0, 0)); constructioninfotest.m_mass = 2000000; constructioninfotest.m_linearDamping = 0; constructioninfotest.m_angularDamping = 0; constructioninfotest.m_friction = 0.1f; constructioninfotest.m_restitution = 0; constructioninfotest.m_linearSleepingThreshold = 0.8f; constructioninfotest.m_angularSleepingThreshold = 1; constructioninfotest.m_additionalDamping = false; constructioninfotest.m_additionalDampingFactor = 0.005f; constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f; constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f; constructioninfotest.m_additionalAngularDampingFactor = 0.01f; constructioninfotest.commit(); btGImpactCollisionAlgorithm.registerAlgorithm(coldisp); btRigidBody cubetest = new btRigidBody(constructioninfotest); dynamicsWorld.addRigidBody(groundbody); dynamicsWorld.addRigidBody(cubetest); dynamicsWorld.addRigidBody(capbody); dynamicsWorld.addRigidBody(meshbody); int frame = 0; for (int i = 0; i < 26; i++) { int frames = dynamicsWorld.stepSimulation(((i % 60) / 60f), 10, (1f / 60)); frame += frames; Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr())); Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr())); Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr())); } dynamicsWorld.removeRigidBody(meshbody); dynamicsWorld.removeRigidBody(capbody); dynamicsWorld.removeRigidBody(cubetest); dynamicsWorld.removeRigidBody(groundbody); cubetest.Dispose(); groundbody.Dispose(); capbody.Dispose(); cap.Dispose(); obj.Dispose(); testbtAxisSweep3.Dispose(); dynamicsWorld.Dispose(); coldisp.Dispose(); colconfig.Dispose(); seqimpconssol.Dispose(); testvec.Dispose(); testvec2.Dispose(); testvec3.Dispose(); testvec4.Dispose(); }
internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis //, int solverBodyIdA, int solverBodyIdB , btSolverBody solverBodyA, btSolverBody solverBodyB , btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 ) { //btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; //btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody body0 = solverBodyA.m_originalBody; btRigidBody body1 = solverBodyB.m_originalBody; solverConstraint.m_solverBodyA = solverBodyA; solverConstraint.m_solverBodyB = solverBodyB; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = null; solverConstraint.m_appliedImpulse = 0; solverConstraint.m_appliedPushImpulse = 0; if( body0 != null ) { solverConstraint.m_contactNormal1 = normalAxis; rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal ); btVector3 tmp; body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp ); tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA ); } else { solverConstraint.m_contactNormal1.setZero(); solverConstraint.m_relpos1CrossNormal.setZero(); solverConstraint.m_angularComponentA.setZero(); } if( body1 != null ) { normalAxis.Invert( out solverConstraint.m_contactNormal2 ); rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal ); btVector3 tmp; body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp ); tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB ); } else { solverConstraint.m_contactNormal2 = btVector3.Zero; solverConstraint.m_relpos2CrossNormal = btVector3.Zero; solverConstraint.m_angularComponentB = btVector3.Zero; } { btVector3 vec; double denom0 = 0; double denom1 = 0; if( body0 != null ) { solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec ); denom0 = body0.getInvMass() + normalAxis.dot( ref vec ); } if( body1 != null ) { btVector3 tmp; solverConstraint.m_angularComponentB.Invert( out tmp ); tmp.cross( ref rel_pos2, out vec ); denom1 = body1.getInvMass() + normalAxis.dot( ref vec ); } double denom = relaxation / ( denom0 + denom1 ); btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) ); solverConstraint.m_jacDiagABInv = denom; } { double rel_vel; double vel1Dotn; double vel2Dotn; //double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero ); if( body0 != null ) vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse ) + solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity ); else vel1Dotn = 0; //double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero ) // + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero ); if( body1 != null ) vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse ) + solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity ); else vel2Dotn = 0; rel_vel = vel1Dotn + vel2Dotn; // double positionalError = 0; double velocityError = desiredVelocity - rel_vel; double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = velocityImpulse; btScalar.Dbg( "Constraint 1 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) ); solverConstraint.m_rhsPenetration = 0; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } }
static void orth( ref btVector3 v, out btVector3 result ) { btVector3 a; v.cross( ref btVector3.Forward, out a ); btVector3 b; v.cross( ref btVector3.yAxis, out b ); if( a.length() > b.length() ) { a.normalized( out result ); } else { b.normalized( out result ); } }
public static double DistanceBetweenLines( ref btVector3 ustart, ref btVector3 udir , ref btVector3 vstart, ref btVector3 vdir , out btVector3 upoint, out btVector3 vpoint ) { btVector3 cp; udir.cross( ref vdir, out cp ); cp.normalize(); double distu = -cp.dot( ref ustart ); double distv = -cp.dot( ref vstart ); double dist = btScalar.btFabs( distu - distv ); btPlane plane; btVector3 tmp; //if( upoint ) { vdir.cross( ref cp, out plane.normal ); plane.normal.normalize(); plane.dist = -plane.normal.dot( ref vstart ); ustart.Add( ref udir, out tmp ); plane.PlaneLineIntersection( ref ustart, ref tmp, out upoint ); } //if( vpoint ) { udir.cross( ref cp, out plane.normal ); plane.normal.normalize(); plane.dist = -plane.normal.dot( ref ustart ); ustart.Add( ref vdir, out tmp ); plane.PlaneLineIntersection( ref vstart, ref tmp, out vpoint ); } return dist; }
/*@brief Return the cross product of two vectors */ public static void btCross( ref btVector3 v1, ref btVector3 v2, out btVector3 result ) { v1.cross( ref v2, out result ); }
/*@brief Return a rotated version of this vector @param wAxis The axis to rotate about @param angle The angle to rotate by */ public void rotate( ref btVector3 wAxis, double angle, out btVector3 result ) { btVector3 o; wAxis.Mult( wAxis.dot( ref this ), out o ); btVector3 _x; this.Sub( ref o, out _x ); btVector3 _y; wAxis.cross( ref this, out _y ); btVector3 tmp; btVector3 tmp2; _x.Mult( btScalar.btCos( angle ), out tmp ); o.Add( ref tmp, out tmp2 ); _y.Mult( btScalar.btSin( angle ), out tmp ); tmp2.Add( ref tmp, out result ); }