public static void Multiply(ref btQuaternion q1, ref btQuaternion q2, out btQuaternion qout) { qout.X = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y; qout.Y = q1.W * q2.Y + q1.Y * q2.W + q1.Z * q2.X - q1.X * q2.Z; qout.Z = q1.W * q2.Z + q1.Z * q2.W + q1.X * q2.Y - q1.Y * q2.X; qout.W = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z; }
public btMatrix3x3(btQuaternion q) { float d = q.Length2; Debug.Assert(d != 0.0f); float s = 2.0f / d; float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; el0 = new btVector3(1.0f - (yy + zz), xy - wz, xz + wy); el1 = new btVector3(xy + wz, 1.0f - (xx + zz), yz - wx); el2 = new btVector3(xz - wy, yz + wx, 1.0f - (xx + yy)); }
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut) { if (m_manifoldPtr == null) return; CollisionObject convexObj = m_isSwapped ? body1 : body0; CollisionObject planeObj = m_isSwapped ? body0 : body1; ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape; StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape; btVector3 planeNormal = planeShape.PlaneNormal; //const btScalar& planeConstant = planeShape->getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { btQuaternion rotq = new btQuaternion(0, 0, 0, 1); collideSingleContact(rotq, body0, body1, dispatchInfo, ref resultOut); } if (resultOut.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold) { btVector3 v0, v1; btVector3.PlaneSpace1(ref planeNormal, out v0, out v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects float angleLimit = 0.125f * BulletGlobal.SIMD_PI; float perturbeAngle; float radius = convexShape.getAngularMotionDisc(); perturbeAngle = PersistentManifold.gContactBreakingThreshold / radius; if (perturbeAngle > angleLimit) perturbeAngle = angleLimit; btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle); for (int i = 0; i < m_numPerturbationIterations; i++) { float iterationAngle = (float)i * (BulletGlobal.SIMD_2_PI / (float)m_numPerturbationIterations); btQuaternion rotq = new btQuaternion(planeNormal, iterationAngle); collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0, body1, dispatchInfo, ref resultOut); } } if (m_ownManifold) { if (m_manifoldPtr.NumContacts != 0) { resultOut.refreshContactPoints(); } } }
public static void integrateTransform(btTransform curTrans,btVector3 linvel, btVector3 angvel, float timeStep,out btTransform predictedTransform) { predictedTransform = btTransform.Identity; #region predictedTransform.Origin=curTrans.Origin + linvel * timeStep; { btVector3 temp; btVector3.Multiply(ref linvel, timeStep, out temp); btVector3.Add(ref curTrans.Origin, ref temp, out predictedTransform.Origin); } #endregion //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia btVector3 axis; float fAngle = angvel.Length; //limit the angular motion if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) { fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } if (fAngle < 0.001f) { // use Taylor's expansions of sync function #region axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle); { btVector3.Multiply(ref angvel, (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle), out axis); } #endregion } else { // sync(fAngle) = sin(c*fAngle)/t #region axis = angvel * ((float)Math.Sin(0.5f * fAngle * timeStep) / fAngle); btVector3.Multiply(ref angvel, ((float)Math.Sin(0.5f * fAngle * timeStep) / fAngle), out axis); #endregion } btQuaternion dorn = new btQuaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(fAngle * timeStep * 0.5f)); btQuaternion orn0 = curTrans.Rotation; btQuaternion predictedOrn; btQuaternion.Multiply(ref dorn, ref orn0, out predictedOrn); predictedOrn.normalize(); predictedTransform.Rotation = predictedOrn; }
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut) { if (m_manifoldPtr == null) { //swapped? m_manifoldPtr = m_dispatcher.getNewManifold(body0, body1); m_ownManifold = true; } resultOut.PersistentManifold = m_manifoldPtr; //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); ConvexShape min0 = (ConvexShape)(body0.CollisionShape); ConvexShape min1 = (ConvexShape)(body1.CollisionShape); btVector3 normalOnB; btVector3 pointOnBWorld; if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE)) { CapsuleShape capsuleA = (CapsuleShape)min0; CapsuleShape capsuleB = (CapsuleShape)min1; btVector3 localScalingA = capsuleA.LocalScaling; btVector3 localScalingB = capsuleB.LocalScaling; float threshold = m_manifoldPtr.ContactBreakingThreshold; float dist = capsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.HalfHeight, capsuleA.Radius, capsuleB.HalfHeight, capsuleB.Radius, capsuleA.UpAxis, capsuleB.UpAxis, body0.WorldTransform, body1.WorldTransform, threshold); if (dist < threshold) { Debug.Assert(normalOnB.Length2 >= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON)); resultOut.addContactPoint(ref normalOnB, ref pointOnBWorld, dist); } resultOut.refreshContactPoints(); return; } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 { ClosestPointInput input; GjkPairDetector gjkPairDetector = new GjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.MinkowskiA = min0; gjkPairDetector.MinkowskiB = min1; #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.ContactBreakingThreshold; input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } //input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0.WorldTransform; input.m_transformB = body1.WorldTransform; gjkPairDetector.getClosestPoints(ref input, ref resultOut, dispatchInfo.m_debugDraw); #if USE_SEPDISTANCE_UTIL2 btScalar sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations != 0 && resultOut.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold) { int i; btVector3 v0, v1; btVector3 sepNormalWorldSpace; //sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); gjkPairDetector.getCachedSeparatingAxis().normalized(out sepNormalWorldSpace); btVector3.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1); bool perturbeA = true; float angleLimit = 0.125f * BulletGlobal.SIMD_PI; float perturbeAngle; float radiusA = min0.getAngularMotionDisc(); float radiusB = min1.getAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusA; perturbeA = true; } else { perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusB; perturbeA = false; } if (perturbeAngle > angleLimit) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for (i = 0; i < m_numPerturbationIterations; i++) { if (v0.Length2 > BulletGlobal.SIMD_EPSILON) { btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle); float iterationAngle = i * (BulletGlobal.SIMD_2_PI / m_numPerturbationIterations); btQuaternion rotq = new btQuaternion(sepNormalWorldSpace, iterationAngle); if (perturbeA) { #region input.m_transformA.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0.WorldTransform.Basis; { btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq); btMatrix3x3.Multiply(ref temp, ref body0.WorldTransform.Basis, out input.m_transformA.Basis); } #endregion input.m_transformB = body1.WorldTransform; #if DEBUG dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformA, 10.0f); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0.WorldTransform; #region input.m_transformB.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1.WorldTransform.Basis; { btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq); btMatrix3x3.Multiply(ref temp, ref body1.WorldTransform.Basis, out input.m_transformB.Basis); } #endregion #if DEBUG dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformB, 10.0f); #endif } PerturbedContactResult perturbedResultOut = new PerturbedContactResult(input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(ref input, ref perturbedResultOut, ref resultOut, dispatchInfo.m_debugDraw); } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut.refreshContactPoints(); } }
/**@brief Return the dot product between this quaternion and another * @param q The other quaternion */ public float dot(btQuaternion q) { return X * q.X + Y * q.Y + Z * q.Z + W * q.W; }
/** @brief Set the matrix from a quaternion * @param q The Quaternion to match */ public void setRotation(ref btQuaternion q) { float d = q.Length2; Debug.Assert(d != 0.0f); float s = 2.0f / d; float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; setValue(1.0f - (yy + zz), xy - wz, xz + wy, xy + wz, 1.0f - (xx + zz), yz - wx, xz - wy, yz + wx, 1.0f - (xx + yy)); }
/**@brief Get the matrix represented as a quaternion * @param q The quaternion which will be set */ public void getRotation(out btQuaternion q) { StackPtr<float> temp = StackPtr<float>.Allocate(4); try { float trace = el0.X + el1.Y + el2.Z; //float[] temp = new float[4]; if (trace > 0.0f) { float s = (float)Math.Sqrt(trace + 1.0); temp[3] = (s * 0.5f); s = 0.5f / s; temp[0] = ((el2.Y - el1.Z) * s); temp[1] = ((el0.Z - el2.X) * s); temp[2] = ((el1.X - el0.Y) * s); } else { int i = el0.X < el1.Y ? (el1.Y < el2.Z ? 2 : 1) : (el0.X < el2.Z ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; float s = (float)Math.Sqrt(this[i][i] - this[j][j] - this[k][k] + 1.0f); temp[i] = s * 0.5f; s = 0.5f / s; temp[3] = (this[k][j] - this[j][k]) * s; temp[j] = (this[j][i] + this[i][j]) * s; temp[k] = (this[k][i] + this[i][k]) * s; } q = new btQuaternion(temp[0], temp[1], temp[2], temp[3]); } finally { temp.Dispose(); } }
public void collideSingleContact(btQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut) { CollisionObject convexObj = m_isSwapped ? body1 : body0; CollisionObject planeObj = m_isSwapped ? body0 : body1; ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape; StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape; bool hasCollision = false; btVector3 planeNormal = planeShape.PlaneNormal; float planeConstant = planeShape.PlaneConstant; btTransform convexWorldTransform = convexObj.WorldTransform; btTransform convexInPlaneTrans; convexInPlaneTrans = planeObj.WorldTransform.inverse() * convexWorldTransform; //now perturbe the convex-world transform #region convexWorldTransform.Basis *= new btMatrix3x3(perturbeRot); { btMatrix3x3 temp1 = convexWorldTransform.Basis, temp2 = new btMatrix3x3(perturbeRot); btMatrix3x3.Multiply(ref temp1, ref temp2, out convexWorldTransform.Basis); } #endregion btTransform planeInConvex; planeInConvex = convexWorldTransform.inverse() * planeObj.WorldTransform; #region btVector3 vtx = convexShape.localGetSupportingVertex(planeInConvex.Basis * -planeNormal); btVector3 vtx; { btVector3 temp, temp2; temp2 = -planeNormal; btMatrix3x3.Multiply(ref planeInConvex.Basis, ref temp2, out temp); //vtx = convexShape.localGetSupportingVertex(temp); convexShape.localGetSupportingVertex(ref temp, out vtx); } #endregion btVector3 vtxInPlane = convexInPlaneTrans * vtx; float distance = (planeNormal.dot(vtxInPlane) - planeConstant); btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObj.WorldTransform * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr.ContactBreakingThreshold; resultOut.PersistentManifold = m_manifoldPtr; if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB;// = planeObj.WorldTransform.Basis * planeNormal; btMatrix3x3.Multiply(ref planeObj.WorldTransform.Basis, ref planeNormal, out normalOnSurfaceB); btVector3 pOnB = vtxInPlaneWorld; resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnB, distance); } }