public void UpdateWheelTransform(int wheelIndex, bool interpolatedTransform) { WheelInfo wheel = m_wheelInfo[wheelIndex]; UpdateWheelTransformsWS(wheel, interpolatedTransform); IndexedVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; IndexedVector3 right = wheel.m_raycastInfo.m_wheelAxleWS; IndexedVector3 fwd = IndexedVector3.Cross(up, right); fwd.Normalize(); // up = right.cross(fwd); // up.normalize(); //rotate around steering over de wheelAxleWS float steering = wheel.m_steering; IndexedQuaternion steeringOrn = new IndexedQuaternion(up, steering); //wheel.m_steering); IndexedBasisMatrix steeringMat = new IndexedBasisMatrix(ref steeringOrn); IndexedQuaternion rotatingOrn = new IndexedQuaternion(right, -wheel.m_rotation); IndexedBasisMatrix rotatingMat = new IndexedBasisMatrix(ref rotatingOrn); IndexedBasisMatrix basis2 = new IndexedBasisMatrix( right.X, fwd.X, up.X, right.Y, fwd.Y, up.Y, right.Z, fwd.Z, up.Z ); // FIXME MAN - MATRIX ORDER //wheel.m_worldTransform = steeringMat * rotatingMat * basis2; wheel.m_worldTransform._basis = steeringMat * rotatingMat * basis2; wheel.m_worldTransform._origin = wheel.m_raycastInfo.m_hardPointWS + (wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); }
public void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB) { IndexedVector3 toPosA = transA._origin; IndexedVector3 toPosB = transB._origin; IndexedQuaternion toOrnA = transA.GetRotation(); IndexedQuaternion toOrnB = transB.GetRotation(); if (m_separatingDistance > 0.0f) { IndexedVector3 linVelA; IndexedVector3 angVelA; IndexedVector3 linVelB; IndexedVector3 angVelB; TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA); TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB); float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB; IndexedVector3 relLinVel = (linVelB - linVelA); float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal); if (relLinVelocLength < 0f) { relLinVelocLength = 0f; } float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength; m_separatingDistance -= projectedMotion; } m_posA = toPosA; m_posB = toPosB; m_ornA = toOrnA; m_ornB = toOrnB; }
public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle) { //IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0)); IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse(); IndexedQuaternion dorn = IndexedQuaternion.Identity; GetRotation(ref dmat, out dorn); ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.Normalize(); angle = MathUtil.QuatAngle(ref dorn); axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z); //axis[3] = float(0.); //check for axis length float len = axis.LengthSquared(); if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON) { axis = new IndexedVector3(1,0,0); } else { axis.Normalize(); } }
public void SetAxis(ref IndexedVector3 axisInA) { IndexedVector3 rbAxisA1, rbAxisA2; TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2); IndexedVector3 pivotInA = m_rbAFrame._origin; // m_rbAFrame._origin = pivotInA; //MathUtil.setBasis(ref m_rbAFrame,ref axisInA,ref rbAxisA1,ref rbAxisA2); m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X, rbAxisA1.Y, rbAxisA2.Y, axisInA.Y, rbAxisA1.Z, rbAxisA2.Z, axisInA.Z); IndexedVector3 axisInB = m_rbA.GetCenterOfMassTransform()._basis *axisInA; IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB); IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1); IndexedVector3 rbAxisB2 = IndexedVector3.Cross(ref axisInB, ref rbAxisB1); m_rbBFrame._origin = m_rbB.GetCenterOfMassTransform().Inverse() * (m_rbA.GetCenterOfMassTransform() * (pivotInA)); m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X, rbAxisB1.Y, rbAxisB2.Y, axisInB.Y, rbAxisB1.Z, rbAxisB2.Z, axisInB.Z); }
public virtual void Rotate(IndexedQuaternion iq) { IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(iq); im._origin = m_graphicsWorldTrans._origin; SetWorldTransform(ref im); }
public static bool ClampNormal(ref IndexedVector3 edge, ref IndexedVector3 tri_normal_org, ref IndexedVector3 localContactNormalOnB, float correctedEdgeAngle, out IndexedVector3 clampedLocalNormal) { IndexedVector3 tri_normal = tri_normal_org; //we only have a local triangle normal, not a local contact normal . only normal in world space... //either compute the current angle all in local space, or all in world space IndexedVector3 edgeCross = IndexedVector3.Cross(edge, tri_normal).Normalized(); float curAngle = GetAngle(ref edgeCross, ref tri_normal, ref localContactNormalOnB); if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { float diffAngle = correctedEdgeAngle - curAngle; IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle); clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB; return(true); } } if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { float diffAngle = correctedEdgeAngle - curAngle; IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle); clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB; return(true); } } clampedLocalNormal = IndexedVector3.Zero; return(false); }
public void SetMotorTarget(ref IndexedQuaternion qAinB, float dt) // qAinB is rotation of body A wrt body B. { // convert target from body to constraint space IndexedQuaternion qConstraint = MathUtil.QuaternionInverse(m_rbBFrame.GetRotation()) * qAinB * m_rbAFrame.GetRotation(); qConstraint.Normalize(); // extract "pure" hinge component IndexedVector3 vNoHinge = MathUtil.QuatRotate(ref qConstraint, ref vHinge); vNoHinge.Normalize(); IndexedQuaternion qNoHinge = MathUtil.ShortestArcQuat(ref vHinge, ref vNoHinge); IndexedQuaternion qHinge = MathUtil.QuaternionInverse(ref qNoHinge) * qConstraint; qHinge.Normalize(); // compute angular target, clamped to limits float targetAngle = MathUtil.QuatAngle(ref qHinge); if (targetAngle > MathUtil.SIMD_PI) // long way around. flip quat and recalculate. { qHinge = -qHinge; targetAngle = MathUtil.QuatAngle(ref qHinge); } if (qHinge.Z < 0) { targetAngle = -targetAngle; } SetMotorTarget(targetAngle, dt); }
internal static bool AlmostEqual(ref IndexedQuaternion v1, ref IndexedQuaternion v2, float nEpsilon) { return ((((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) && (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) && (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) && (((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon)))); }
public EntityProperties FromTransform(uint id, IndexedMatrix startTransform) { EntityProperties ret = new EntityProperties(); ID = id; Position = startTransform._origin; Rotation = startTransform.GetRotation(); return(ret); }
void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB) { m_separatingNormal = separatingVector; m_separatingDistance = separatingDistance; IndexedVector3 toPosA = transA._origin; IndexedVector3 toPosB = transB._origin; IndexedQuaternion toOrnA = transA.GetRotation(); IndexedQuaternion toOrnB = transB.GetRotation(); m_posA = toPosA; m_posB = toPosB; m_ornA = toOrnA; m_ornB = toOrnB; }
public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform) { predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep); // #define QUATERNION_DERIVATIVE #if QUATERNION_DERIVATIVE IndexedVector3 pos; IndexedQuaternion predictedOrn; IndexedVector3 scale; curTrans.Decompose(ref scale, ref predictedOrn, ref pos); predictedOrn += (angvel * predictedOrn) * (timeStep * .5f)); predictedOrn.Normalize(); #else //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia IndexedVector3 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 axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle ); } else { // sync(fAngle) = sin(c*fAngle)/t axis = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle ); } IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) ); IndexedQuaternion orn0 = curTrans.GetRotation(); IndexedQuaternion predictedOrn = dorn * orn0; predictedOrn.Normalize(); #endif IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn); predictedTransform._basis = newMatrix._basis; }
public HingeConstraint(RigidBody rbA, ref IndexedVector3 pivotInA, ref IndexedVector3 axisInA, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA) { m_angularOnly = false; m_enableAngularMotor = false; m_useReferenceFrameA = useReferenceFrameA; m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET; m_flags = 0; #if _BT_USE_CENTER_LIMIT_ m_limit = new AngularLimit(); #endif // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace IndexedVector3 rbAxisA1, rbAxisA2; TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2); m_rbAFrame._origin = pivotInA; //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA); m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X, rbAxisA1.Y, rbAxisA2.Y, axisInA.Y, rbAxisA1.Z, rbAxisA2.Z, axisInA.Z); IndexedVector3 axisInB = rbA.GetCenterOfMassTransform()._basis *axisInA; IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB); IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1); IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1); m_rbBFrame._origin = rbA.GetCenterOfMassTransform() * pivotInA; //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB); m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X, rbAxisB1.Y, rbAxisB2.Y, axisInB.Y, rbAxisB1.Z, rbAxisB2.Z, axisInB.Z); //start with free #if !_BT_USE_CENTER_LIMIT_ m_lowerLimit = 1f; m_upperLimit = -1f; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f; }
public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle) { IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a); IndexedQuaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0); ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.Normalize(); angle = MathUtil.QuatAngle(ref dorn); axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z); //check for axis length float len = axis.LengthSquared(); if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON) { axis = new IndexedVector3(1f, 0, 0); } else { axis.Normalize(); } }
public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { CollisionObject convexObj = m_isSwapped ? body1 : body0; CollisionObject planeObj = m_isSwapped ? body0 : body1; ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape; StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape; bool hasCollision = false; IndexedVector3 planeNormal = planeShape.GetPlaneNormal(); float planeConstant = planeShape.GetPlaneConstant(); IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform(); IndexedMatrix convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexWorldTransform; //now perturbe the convex-world transform convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot); IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform();; IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal); IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx; float distance = (IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant); IndexedVector3 vtxInPlaneProjected = vtxInPlane - (distance * planeNormal); IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold(); resultOut.SetPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal; IndexedVector3 pOnB = vtxInPlaneWorld; resultOut.AddContactPoint(ref normalOnSurfaceB, ref pOnB, distance); } }
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex) { //skip self-collisions if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) { return; } //skip duplicates (disabled for now) //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) // return; //search for shared vertices and edges int numshared = 0; int[] sharedVertsA = new int[] { -1, -1, -1 }; int[] sharedVertsB = new int[] { -1, -1, -1 }; ///skip degenerate triangles float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared(); if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold) { return; } float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared(); ///skip degenerate triangles if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold) { return; } #if false printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", m_triangleVerticesA[0].GetX(), m_triangleVerticesA[0].GetY(), m_triangleVerticesA[0].GetZ(), m_triangleVerticesA[1].GetX(), m_triangleVerticesA[1].GetY(), m_triangleVerticesA[1].GetZ(), m_triangleVerticesA[2].GetX(), m_triangleVerticesA[2].GetY(), m_triangleVerticesA[2].GetZ()); printf("partId=%d, triangleIndex=%d\n", partId, triangleIndex); printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", triangle[0].GetX(), triangle[0].GetY(), triangle[0].GetZ(), triangle[1].GetX(), triangle[1].GetY(), triangle[1].GetZ(), triangle[2].GetX(), triangle[2].GetY(), triangle[2].GetZ()); #endif for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case if (numshared >= 3) { return; } } } ///degenerate case if (numshared >= 3) { return; } } switch (numshared) { case 0: { break; } case 1: { //shared vertex break; } case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) { sharedVertsA[0] = 2; sharedVertsA[1] = 0; int tmp = sharedVertsB[1]; sharedVertsB[1] = sharedVertsB[0]; sharedVertsB[0] = tmp; } int hash = GetHash(m_partIdA, m_triangleIndexA); TriangleInfo info = null; if (m_triangleInfoMap.ContainsKey(hash)) { info = m_triangleInfoMap[hash]; } else { info = new TriangleInfo(); m_triangleInfoMap[hash] = info; } int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; int otherIndexA = 3 - sumvertsA; IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); IndexedVector3 normalA; IndexedVector3 normalB; tA.CalcNormal(out normalA); tB.CalcNormal(out normalB); edge.Normalize(); IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA)); { IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (IndexedVector3.Dot(edgeCrossA, tmp) < 0) { edgeCrossA *= -1; } } IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized(); { IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (IndexedVector3.Dot(edgeCrossB, tmp) < 0) { edgeCrossB *= -1; } } float angle2 = 0; float ang4 = 0.0f; IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB); float len2 = calculatedEdge.LengthSquared(); float correctedAngle = 0f; IndexedVector3 calculatedNormalB = normalA; bool isConvex = false; if (len2 < m_triangleInfoMap.m_planarEpsilon) { angle2 = 0.0f; ang4 = 0.0f; } else { calculatedEdge.Normalize(); IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA); calculatedNormalA.Normalize(); angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB); ang4 = MathUtil.SIMD_PI - angle2; float dotA = IndexedVector3.Dot(normalA, edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision isConvex = (dotA < 0f); correctedAngle = isConvex ? ang4 : -ang4; IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle); IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2); calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA; } //alternatively use //IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA); switch (sumvertsA) { case 1: { IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); float bla = IndexedVector3.Dot(computedNormalB, normalB); if (bla < 0) { computedNormalB *= -1; info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001f) { System.Console.WriteLine("warning: normals not identical"); } #endif//DEBUG_INTERNAL_EDGE info.m_edgeV0V1Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX; } break; } case 2: { IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); if (IndexedVector3.Dot(computedNormalB, normalB) < 0) { computedNormalB *= -1; info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001) { System.Console.WriteLine("warning: normals not identical"); } #endif //DEBUG_INTERNAL_EDGE info.m_edgeV2V0Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX; } break; } case 3: { IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); if (IndexedVector3.Dot(computedNormalB, normalB) < 0) { info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB; computedNormalB *= -1; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001) { System.Console.WriteLine("warning: normals not identical"); } #endif //DEBUG_INTERNAL_EDGE info.m_edgeV1V2Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX; } break; } } break; } default: { // printf("warning: duplicate triangle\n"); break; } } }
/// Changes a btManifoldPoint collision normal to the normal from the mesh. public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags) { //btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE) { return; } BvhTriangleMeshShape trimesh = null; if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) { //trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape(); } else { trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape(); } TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap(); if (triangleInfoMapPtr == null) { return; } int hash = GetHash(partId0, index0); TriangleInfo info; if (!triangleInfoMapPtr.TryGetValue(hash, out info)) { return; } float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f; TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape; IndexedVector3 v0, v1, v2; tri_shape.GetVertex(0, out v0); tri_shape.GetVertex(1, out v1); tri_shape.GetVertex(2, out v2); IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f); IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0); IndexedVector3 tri_normal; tri_shape.CalcNormal(out tri_normal); //float dot = tri_normal.dot(cp.m_normalWorldOnB); IndexedVector3 nearest; NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest); IndexedVector3 contact = cp.m_localPointB; #if BT_INTERNAL_EDGE_DEBUG_DRAW IndexedMatrix tr = colObj0.GetWorldTransform(); DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; int numConcaveEdgeHits = 0; int numConvexEdgeHits = 0; IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB; localContactNormalOnB.Normalize();//is this necessary? // Get closest edge int bestedge = -1; float disttobestedge = MathUtil.BT_LARGE_FLOAT; // // Edge 0 . 1 if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold) { //IndexedVector3 nearest; NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest); float len = (contact - nearest).Length(); // if (len < disttobestedge) { bestedge = 0; disttobestedge = len; } } // Edge 1 . 2 if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold) { //IndexedVector3 nearest; NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest); float len = (contact - nearest).Length(); // if (len < disttobestedge) { bestedge = 1; disttobestedge = len; } } // Edge 2 . 0 if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold) { //IndexedVector3 nearest; NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest); float len = (contact - nearest).Length(); // if (len < disttobestedge) { bestedge = 2; disttobestedge = len; } } #if BT_INTERNAL_EDGE_DEBUG_DRAW IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f); DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red); #endif if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold) { #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); #endif float len = (contact - nearest).Length(); if (len < triangleInfoMapPtr.m_edgeDistanceThreshold) { if (bestedge == 0) { IndexedVector3 edge = (v0 - v1); isNearEdge = true; if (info.m_edgeV0V1Angle == 0.0f) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0; float swapFactor = isEdgeConvex ? 1.0f : -1.0f; #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW IndexedVector3 nA = swapFactor * tri_normal; IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal); if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0) { computedNormalB *= -1; } IndexedVector3 nB = swapFactor * computedNormalB; float NdotA = localContactNormalOnB.Dot(ref nA); float NdotB = localContactNormalOnB.Dot(ref nB); bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon); #if DEBUG_INTERNAL_EDGE { DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red); } #endif //DEBUG_INTERNAL_EDGE if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; IndexedVector3 clampedLocalNormal; bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0)) { IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. (what about cp.m_distance1?) cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB); } } } } } } } NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest); #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green); #endif if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold) { #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW float len = (contact - nearest).Length(); if (len < triangleInfoMapPtr.m_edgeDistanceThreshold) { if (bestedge == 1) { isNearEdge = true; #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW IndexedVector3 edge = (v1 - v2); isNearEdge = true; if (info.m_edgeV1V2Angle == 0f) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0; float swapFactor = isEdgeConvex ? 1.0f : -1.0f; #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW IndexedVector3 nA = swapFactor * tri_normal; IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal); if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0) { computedNormalB *= -1; } IndexedVector3 nB = swapFactor * computedNormalB; #if DEBUG_INTERNAL_EDGE { DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red); } #endif //DEBUG_INTERNAL_EDGE float NdotA = localContactNormalOnB.Dot(ref nA); float NdotB = localContactNormalOnB.Dot(ref nB); bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon); if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB; IndexedVector3 clampedLocalNormal; bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0)) { IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB); } } } } } } } NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest); #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue); #endif if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold) { #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW float len = (contact - nearest).Length(); if (len < triangleInfoMapPtr.m_edgeDistanceThreshold) { if (bestedge == 2) { isNearEdge = true; #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW IndexedVector3 edge = (v2 - v0); if (info.m_edgeV2V0Angle == 0f) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0; float swapFactor = isEdgeConvex ? 1.0f : -1.0f; #if BT_INTERNAL_EDGE_DEBUG_DRAW DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW IndexedVector3 nA = swapFactor * tri_normal; IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal); if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0) { computedNormalB *= -1; } IndexedVector3 nB = swapFactor * computedNormalB; #if DEBUG_INTERNAL_EDGE { DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red); } #endif //DEBUG_INTERNAL_EDGE float NdotA = localContactNormalOnB.Dot(ref nA); float NdotB = localContactNormalOnB.Dot(ref nB); bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon); if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; // printf("hitting convex edge\n"); IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB; IndexedVector3 clampedLocalNormal; bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0)) { IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB); } } } } } } } #if DEBUG_INTERNAL_EDGE { IndexedVector3 color = new IndexedVector3(0, 1, 1); DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color); } #endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { if (numConcaveEdgeHits > 0) { if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.Dot(ref localContactNormalOnB) < 0) { tri_normal *= -1; } cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *tri_normal; } else { IndexedVector3 newNormal = tri_normal * frontFacing; //if the tri_normal is pointing opposite direction as the current local contact normal, skip it float d = newNormal.Dot(ref localContactNormalOnB); if (d < 0) { return; } //modify the normal to be the triangle normal (or backfacing normal) cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *newNormal; } // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB); } } }
public static void CalculateVelocityQuaternion(ref IndexedVector3 pos0, ref IndexedVector3 pos1, ref IndexedQuaternion orn0, ref IndexedQuaternion orn1, float timeStep, out IndexedVector3 linVel, out IndexedVector3 angVel) { linVel = (pos1 - pos0) / timeStep; if (orn0 != orn1) { IndexedVector3 axis; float angle; CalculateDiffAxisAngleQuaternion(ref orn0, ref orn1, out axis, out angle); angVel = axis * (angle / timeStep); } else { angVel = IndexedVector3.Zero; } }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (m_manifoldPtr == null) { return; } CollisionObject convexObj = m_isSwapped ? body1 : body0; CollisionObject planeObj = m_isSwapped ? body0 : body1; ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape; StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape; bool hasCollision = false; IndexedVector3 planeNormal = planeShape.GetPlaneNormal(); float planeConstant = planeShape.GetPlaneConstant(); IndexedMatrix planeInConvex; planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform(); IndexedMatrix convexInPlaneTrans; convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform(); IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal); IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx; float distance = (planeNormal.Dot(vtxInPlane) - planeConstant); IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold(); resultOut.SetPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal; IndexedVector3 pOnB = vtxInPlaneWorld; resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance); } ////first perform a collision query with the non-perturbated collision objects //{ // IndexedQuaternion rotq = IndexedQuaternion.Identity; // CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut); //} if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold) { IndexedVector3 v0; IndexedVector3 v1; TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects float angleLimit = 0.125f * MathUtil.SIMD_PI; float perturbeAngle; float radius = convexShape.GetAngularMotionDisc(); perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius; if (perturbeAngle > angleLimit) { perturbeAngle = angleLimit; } IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle); for (int i = 0; i < m_numPerturbationIterations; i++) { float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); IndexedQuaternion rotq = new IndexedQuaternion(planeNormal, iterationAngle); rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq; CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut); } } if (m_ownManifold) { if (m_manifoldPtr.GetNumContacts() > 0) { resultOut.RefreshContactPoints(); } } }
public static void GetRotation(ref IndexedBasisMatrix a, out IndexedQuaternion rot) { rot = a.GetRotation(); }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (m_manifoldPtr == null) { //swapped? m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1); m_ownManifold = true; } //resultOut = new ManifoldResult(); resultOut.SetPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut.GetPersistentManifold().ClearManifold(); ConvexShape min0 = body0.GetCollisionShape() as ConvexShape; ConvexShape min1 = body1.GetCollisionShape() as ConvexShape; IndexedVector3 normalOnB; IndexedVector3 pointOnBWorld; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE)) { CapsuleShape capsuleA = min0 as CapsuleShape; CapsuleShape capsuleB = min1 as CapsuleShape; //IndexedVector3 localScalingA = capsuleA.GetLocalScaling(); //IndexedVector3 localScalingB = capsuleB.GetLocalScaling(); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(), capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(), body0.GetWorldTransform(), body1.GetWorldTransform(), threshold); if (dist < threshold) { Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)); resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist); } resultOut.RefreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #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 = ClosestPointInput.Default(); using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get()) { gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.SetMinkowskiA(min0); gjkPairDetector.SetMinkowskiB(min1); #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = float.MaxValue; } else #endif //USE_SEPDISTANCE_UTIL2 { input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold(); input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } //input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); if (min0.IsPolyhedral() && min1.IsPolyhedral()) { DummyResult dummy = new DummyResult(); PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape; PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape; if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null) { float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float minDist = float.MinValue; IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); bool foundSepAxis = true; if (dispatchInfo.m_enableSatConvex) { foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis( polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), out sepNormalWorldSpace); } else { #if ZERO_MARGIN gjkPairDetector.SetIgnoreMargin(true); gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw); #endif float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared(); if (l2 > MathUtil.SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin(); #if ZERO_MARGIN foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f; #else foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin()); #endif } } if (foundSepAxis) { // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), minDist - threshold, threshold, resultOut); } if (m_ownManifold) { resultOut.RefreshContactPoints(); } return; } else { //we can also deal with convex versus triangle (without connectivity data) if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE) { m_vertices.Clear(); TriangleShape tri = polyhedronB as TriangleShape; m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]); m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]); m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);; float minDist = float.MinValue; float maxDist = threshold; bool foundSepAxis = false; if (false) { polyhedronB.InitializePolyhedralFeatures(); foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis( polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), out sepNormalWorldSpace); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); } else { #if ZERO_MARGIN gjkPairDetector.SetIgnoreMargin(true); gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared(); if (l2 > MathUtil.SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin(); foundSepAxis = true; } } if (foundSepAxis) { PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut); } if (m_ownManifold) { resultOut.RefreshContactPoints(); } return; } } } gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false); #if USE_SEPDISTANCE_UTIL2 float sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist > MathUtil.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.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold) { IndexedVector3 v0, v1; IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis(); sepNormalWorldSpace.Normalize(); TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1); bool perturbeA = true; const float angleLimit = 0.125f * MathUtil.SIMD_PI; float perturbeAngle; float radiusA = min0.GetAngularMotionDisc(); float radiusB = min1.GetAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA; perturbeA = true; } else { perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB; perturbeA = false; } if (perturbeAngle > angleLimit) { perturbeAngle = angleLimit; } IndexedMatrix unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for (int i = 0; i < m_numPerturbationIterations; i++) { if (v0.LengthSquared() > MathUtil.SIMD_EPSILON) { IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle); float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle); if (perturbeA) { input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis); input.m_transformB = body1.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0.GetWorldTransform(); input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f); #endif } PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw()); gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false); } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 } } if (m_ownManifold) { resultOut.RefreshContactPoints(); } }
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB) { m_angularOnly = false; m_enableAngularMotor = false; m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET; m_useReferenceFrameA = useReferenceFrameA; m_rbAFrame._origin = pivotInA; #if _BT_USE_CENTER_LIMIT_ m_limit = new AngularLimit(); #endif m_flags = 0; // since no frame is given, assume this to be zero angle and just pick rb transform axis IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0); IndexedVector3 rbAxisA2 = IndexedVector3.Zero; float projection = IndexedVector3.Dot(axisInA, rbAxisA1); if (projection >= 1.0f - MathUtil.SIMD_EPSILON) { rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2); rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1); } else if (projection <= -1.0f + MathUtil.SIMD_EPSILON) { rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2); rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1); } else { rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1); rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA); } //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA); m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X, rbAxisA1.Y, rbAxisA2.Y, axisInA.Y, rbAxisA1.Z, rbAxisA2.Z, axisInA.Z); IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB); IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1); IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1); m_rbBFrame._origin = pivotInB; //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB); m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X, rbAxisB1.Y, rbAxisB2.Y, axisInB.Y, rbAxisB1.Z, rbAxisB2.Z, axisInB.Z); #if !_BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = float(1.0f); m_upperLimit = float(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? -1f : 1f; }