public static void Multiply(ref btVector3 v, ref btMatrix3x3 m, out btVector3 result) { result.X = m.tdotx(ref v); result.Y = m.tdoty(ref v); result.Z = m.tdotz(ref v); result.W = 0; }
public static void Multiply(ref btMatrix3x3 m, ref btVector3 v, out btVector3 result) { //result = new btVector3(); result.X = m.el0.dot(v); result.Y = m.el1.dot(v); result.Z = m.el2.dot(v); result.W = 0; }
/*public static btMatrix3x3 operator *(btMatrix3x3 m1, btMatrix3x3 m2) { return new btMatrix3x3( m2.tdotx(m1.el0), m2.tdoty(m1.el0), m2.tdotz(m1.el0), m2.tdotx(m1.el1), m2.tdoty(m1.el1), m2.tdotz(m1.el1), m2.tdotx(m1.el2), m2.tdoty(m1.el2), m2.tdotz(m1.el2)); }*/ public static void Multiply(ref btMatrix3x3 m1, ref btMatrix3x3 m2, out btMatrix3x3 result) { result.el0.X = m2.tdotx(ref m1.el0); result.el0.Y = m2.tdoty(ref m1.el0); result.el0.Z = m2.tdotz(ref m1.el0); result.el0.W = 0; result.el1.X = m2.tdotx(ref m1.el1); result.el1.Y = m2.tdoty(ref m1.el1); result.el1.Z = m2.tdotz(ref m1.el1); result.el1.W = 0; result.el2.X = m2.tdotx(ref m1.el2); result.el2.Y = m2.tdoty(ref m1.el2); result.el2.Z = m2.tdotz(ref m1.el2); result.el2.W = 0; }
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(); } }
/*public btMatrix3x3 transposeTimes(btMatrix3x3 m) { return new btMatrix3x3( el0.X * m.el0.X + el1.X * m.el1.X + el2.X * m.el2.X, el0.X * m.el0.Y + el1.X * m.el1.Y + el2.X * m.el2.Y, el0.X * m.el0.Z + el1.X * m.el1.Z + el2.X * m.el2.Z, el0.Y * m.el0.X + el1.Y * m.el1.X + el2.Y * m.el2.X, el0.Y * m.el0.Y + el1.Y * m.el1.Y + el2.Y * m.el2.Y, el0.Y * m.el0.Z + el1.Y * m.el1.Z + el2.Y * m.el2.Z, el0.Z * m.el0.X + el1.Z * m.el1.X + el2.Z * m.el2.X, el0.Z * m.el0.Y + el1.Z * m.el1.Y + el2.Z * m.el2.Y, el0.Z * m.el0.Z + el1.Z * m.el1.Z + el2.Z * m.el2.Z); }*/ public void transposeTimes(ref btMatrix3x3 m, out btMatrix3x3 result) { result.el0.X = el0.X * m.el0.X + el1.X * m.el1.X + el2.X * m.el2.X; result.el0.Y = el0.X * m.el0.Y + el1.X * m.el1.Y + el2.X * m.el2.Y; result.el0.Z = el0.X * m.el0.Z + el1.X * m.el1.Z + el2.X * m.el2.Z; result.el0.W = 0; result.el1.X = el0.Y * m.el0.X + el1.Y * m.el1.X + el2.Y * m.el2.X; result.el1.Y = el0.Y * m.el0.Y + el1.Y * m.el1.Y + el2.Y * m.el2.Y; result.el1.Z = el0.Y * m.el0.Z + el1.Y * m.el1.Z + el2.Y * m.el2.Z; result.el1.W = 0; result.el2.X = el0.Z * m.el0.X + el1.Z * m.el1.X + el2.Z * m.el2.X; result.el2.Y = el0.Z * m.el0.Y + el1.Z * m.el1.Y + el2.Z * m.el2.Y; result.el2.Z = el0.Z * m.el0.Z + el1.Z * m.el1.Z + el2.Z * m.el2.Z; result.el2.W = 0; }
/*public btMatrix3x3 inverse() { btVector3 co=new btVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); float det = el0.dot(co); Debug.Assert(det != 0.0); float s = 1.0f / det; return new btMatrix3x3(co.X * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co.Y * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, co.Z * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); }*/ public void inverse(out btMatrix3x3 result) { btVector3 co = new btVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); float det = el0.dot(co); Debug.Assert(det != 0.0); float s = 1.0f / det; result = new btMatrix3x3(co.X * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co.Y * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, co.Z * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); }
/*public btMatrix3x3 absolute() { return new btMatrix3x3( Math.Abs(el0.X), Math.Abs(el0.Y), Math.Abs(el0.Z), Math.Abs(el1.X), Math.Abs(el1.Y), Math.Abs(el1.Z), Math.Abs(el2.X), Math.Abs(el2.Y), Math.Abs(el2.Z)); }*/ public void absolute(out btMatrix3x3 result) { result.el0.X = Math.Abs(el0.X); result.el0.Y = Math.Abs(el0.Y); result.el0.Z = Math.Abs(el0.Z); result.el0.W = 0; result.el1.X = Math.Abs(el1.X); result.el1.Y = Math.Abs(el1.Y); result.el1.Z = Math.Abs(el1.Z); result.el1.W = 0; result.el2.X = Math.Abs(el2.X); result.el2.Y = Math.Abs(el2.Y); result.el2.Z = Math.Abs(el2.Z); result.el2.W = 0; }
/*public btMatrix3x3 scaled(btVector3 s) { return new btMatrix3x3(el0.X * s.X, el0.Y * s.Y, el0.Z * s.Z, el1.X * s.X, el1.Y * s.Y, el1.Z * s.Z, el2.X * s.X, el2.Y * s.Y, el2.Z * s.Z); }*/ public void scaled(ref btVector3 s, out btMatrix3x3 result) { result.el0.X = el0.X * s.X; result.el0.Y = el0.Y * s.Y; result.el0.Z = el0.Z * s.Z; result.el0.W = 0; result.el1.X = el1.X * s.X; result.el1.Y = el1.Y * s.Y; result.el1.Z = el1.Z * s.Z; result.el1.W = 0; result.el2.X = el2.X * s.X; result.el2.Y = el2.Y * s.Y; result.el2.Z = el2.Z * s.Z; result.el2.W = 0; }
/*public btMatrix3x3 transpose() { return new btMatrix3x3(el0.X, el1.X, el2.X, el0.Y, el1.Y, el2.Y, el0.Z, el1.Z, el2.Z); }*/ public void transpose(out btMatrix3x3 result) { result.el0.X = el0.X; result.el0.Y = el1.X; result.el0.Z = el2.X; result.el0.W = 0; result.el1.X = el0.Y; result.el1.Y = el1.Y; result.el1.Z = el2.Y; result.el1.W = 0; result.el2.X = el0.Z; result.el2.Y = el1.Z; result.el2.Z = el2.Z; result.el2.W = 0; }
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); } }