protected override bool Interact(bool staticCollision) { if (!staticCollision && GetRigidBody1().IsStatic() && GetRigidBody2().IsStatic()) { return(false); } MyRBSphereElement sphere1 = (MyRBSphereElement)RBElement1; MyRBSphereElement sphere2 = (MyRBSphereElement)RBElement2; Matrix matrix1 = sphere1.GetGlobalTransformation(); Matrix matrix2 = sphere2.GetGlobalTransformation(); Vector3 p1 = matrix1.Translation; Vector3 p2 = matrix2.Translation; Vector3 d = p2 - p1; float length = d.Length(); float contactRadius = sphere1.Radius + sphere2.Radius; float eps = MyPhysics.physicsSystem.GetRigidBodyModule().CollisionEpsilon; if (staticCollision) { return(length < contactRadius); } // from now on we handle dynamic collision float dynEps = 0; if (!staticCollision && length > eps) { dynEps = Vector3.Dot(GetRigidBody1().LinearVelocity - GetRigidBody2().LinearVelocity, d) / length * MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep; if (dynEps < 0) { dynEps = 0; } } if (length > MyMwcMathConstants.EPSILON && length < contactRadius + eps + dynEps) { Vector3 n = MyMwcUtils.Normalize(d); Vector3 p = p1 + n * (sphere1.Radius + (length - contactRadius) * 0.5f); float error = length - (contactRadius + 0.5f * eps); MySmallCollPointInfo[] collInfo = MyContactInfoCache.SCPIStackAlloc(); collInfo[0] = new MySmallCollPointInfo(p - matrix1.Translation, p - matrix2.Translation, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, n, error, p); MyPhysics.physicsSystem.GetContactConstraintModule().AddContactConstraint(this, collInfo, 1); MyContactInfoCache.FreeStackAlloc(collInfo); return(true); } return(false); }
public override void DoWork() { MyRBSphereElement sphere = (MyRBSphereElement)m_RBElement; MyBoxSensorElement box = (MyBoxSensorElement)m_SensorElement; Matrix boxMatrix = box.GetGlobalTransformation(); Vector3 sphereCenter = sphere.GetGlobalTransformation().Translation; Matrix invBoxMatrix = Matrix.Invert(boxMatrix); Vector3 boxLocalsphereCenter = Vector3.Transform(sphereCenter, invBoxMatrix); bool penetration = false; Vector3 normal = new Vector3(); Vector3 closestPos = new Vector3(); uint customData = 0; MyElementHelper.GetClosestPointForBox(box.Extent, boxLocalsphereCenter, ref closestPos, ref normal, ref penetration, ref customData); if (penetration) { m_IsInside = true; return; } closestPos = Vector3.Transform(closestPos, boxMatrix); float vLength = (sphereCenter - closestPos).LengthSquared(); if (vLength <= sphere.Radius * sphere.Radius) { m_IsInside = true; } else { m_IsInside = false; } }
protected override bool Interact(bool staticCollision) { if (!staticCollision && GetRigidBody1().IsStatic() && GetRigidBody2().IsStatic()) { return(false); } MyRBBoxElement box = null; MyRBSphereElement sphere = null; if (RBElement1.GetElementType() == MyRBElementType.ET_BOX) { SwapElements(); } box = (MyRBBoxElement)RBElement2; sphere = (MyRBSphereElement)RBElement1; Matrix boxMatrix = box.GetGlobalTransformation(); Vector3 sphereCenter = sphere.GetGlobalTransformation().Translation; Matrix invBoxMatrix = Matrix.Invert(boxMatrix); Vector3 boxLocalsphereCenter = Vector3.Transform(sphereCenter, invBoxMatrix); bool penetration = false; Vector3 normal = new Vector3(); Vector3 closestPos = new Vector3(); uint customData = 0; box.GetClosestPoint(boxLocalsphereCenter, ref closestPos, ref normal, ref penetration, ref customData); closestPos = Vector3.Transform(closestPos, boxMatrix); normal = -Vector3.TransformNormal(normal, boxMatrix); normal = MyMwcUtils.Normalize(normal); float vLength = (sphereCenter - closestPos).Length(); if (staticCollision) { return(vLength > 0 && vLength < sphere.Radius); } else { float eps = MyPhysics.physicsSystem.GetRigidBodyModule().CollisionEpsilon; float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep; Vector3 pointVelocity1 = new Vector3(); Vector3 pointVelocity2 = new Vector3(); GetRigidBody1().GetGlobalPointVelocity(ref closestPos, out pointVelocity1); GetRigidBody2().GetGlobalPointVelocity(ref closestPos, out pointVelocity2); float dynEps = 0; if (vLength >= eps) { float dot = Vector3.Dot(pointVelocity1 - pointVelocity2, normal) * dt; if (dot >= 0) { dynEps = dot; } } float radius = sphere.Radius; //Second part of condition commented due to 5968: Bug B - rocket passing through prefab //Does not seem to have any reason to be there if (vLength > 0 /*&& vLength < (radius + eps + dynEps)*/) { float error = vLength - (radius + 0.5f * eps); //error = System.Math.Min(error, eps); MySmallCollPointInfo[] collInfo = MyContactInfoCache.SCPIStackAlloc(); collInfo[0] = new MySmallCollPointInfo(closestPos - sphereCenter, closestPos - boxMatrix.Translation, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, normal, error, closestPos); MyPhysics.physicsSystem.GetContactConstraintModule().AddContactConstraint(this, collInfo, 1); MyContactInfoCache.FreeStackAlloc(collInfo); } } return(false); }
public MyRBElement CreateRBElement(MyRBElementDesc desc) { switch (desc.GetElementType()) { case MyRBElementType.ET_SPHERE: { MyRBSphereElement element = m_RBSphereElementPool.Allocate(); MyCommonDebugUtils.AssertDebug(element != null); if (element.LoadFromDesc(desc)) { return(element); } else { m_RBSphereElementPool.Deallocate(element); return(null); } } break; case MyRBElementType.ET_BOX: { MyRBBoxElement element = m_RBBoxElementPool.Allocate(); MyCommonDebugUtils.AssertDebug(element != null); if (element.LoadFromDesc(desc)) { return(element); } else { m_RBBoxElementPool.Deallocate(element); return(null); } } break; case MyRBElementType.ET_CAPSULE: { MyRBCapsuleElement element = m_RBCapsuleElementPool.Allocate(); MyCommonDebugUtils.AssertDebug(element != null); if (element.LoadFromDesc(desc)) { return(element); } else { m_RBCapsuleElementPool.Deallocate(element); return(null); } } break; case MyRBElementType.ET_TRIANGLEMESH: { MyRBTriangleMeshElement element = m_RBTriangleMeshElementPool.Allocate(); MyCommonDebugUtils.AssertDebug(element != null); if (element.LoadFromDesc(desc)) { return(element); } else { m_RBTriangleMeshElementPool.Deallocate(element); return(null); } } break; case MyRBElementType.ET_VOXEL: { MyRBVoxelElement element = m_RBVoxelElementPool.Allocate(); MyCommonDebugUtils.AssertDebug(element != null); if (element.LoadFromDesc(desc)) { return(element); } else { m_RBVoxelElementPool.Deallocate(element); return(null); } } break; default: // unknown element type MyCommonDebugUtils.AssertDebug(false); break; } return(null); }