/// <summary> /// Constructor /// </summary> /// <param name="n"></param> /// <param name="d"></param> public Plane(Vector3 n, float d) : base((int)PrimitiveType.Plane) { JiggleMath.NormalizeSafe(ref n); this.normal = n; this.d = d; }
/// <summary> /// Constructor /// </summary> /// <param name="n"></param> /// <param name="pos"></param> public Plane(Vector3 n, Vector3 pos) : base((int)PrimitiveType.Plane) { JiggleMath.NormalizeSafe(ref n); this.normal = n; this.d = -Vector3.Dot(n, pos); }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; var newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; var oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; var newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; var oldDist = oldBox.GetDistanceToPoint(out var oldBoxPoint, oldSphere.Position); var newDist = newBox.GetDistanceToPoint(out var newBoxPoint, newSphere.Position); var oldDepth = oldSphere.Radius - oldDist; var newDepth = newSphere.Radius - newDist; if (System.Math.Max(oldDepth, newDepth) > -collTolerance) { Vector3 dir; if (oldDist < -JiggleMath.Epsilon) { dir = oldBoxPoint - oldSphere.Position - oldBoxPoint; JiggleMath.NormalizeSafe(ref dir); } else if (oldDist > JiggleMath.Epsilon) { dir = oldSphere.Position - oldBoxPoint; JiggleMath.NormalizeSafe(ref dir); } else { dir = oldSphere.Position - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } unsafe { var collInfo = new SmallCollPointInfo(oldBoxPoint - body0Pos, oldBoxPoint - body1Pos, oldDepth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }
/// <summary> /// UpdateController /// </summary> /// <param name="dt"></param> public override void UpdateController(float dt) { if (body0 == null || body1 == null) { return; } //Assert(0 != mBody0); //Assert(0 != mBody1); if (damping > 0.0f) { // Some hinges can bend in wonky ways. Derive the effective hinge axis // using the relative rotation of the bodies. Vector3 hingeAxis = body1.AngularVelocity - body0.AngularVelocity; JiggleMath.NormalizeSafe(ref hingeAxis); float angRot1;// Vector3.Dot(ref body0.transformRate.AngularVelocity, ref hingeAxis, out angRot1); float angRot2; Vector3.Dot(ref body1.transformRate.AngularVelocity, ref hingeAxis, out angRot2); float avAngRot = 0.5f * (angRot1 + angRot2); float frac = 1.0f - damping; float newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac; float newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac; Vector3 newAngVel1;// = body0.AngVel + (newAngRot1 - angRot1) * hingeAxis; Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out newAngVel1); Vector3.Add(ref newAngVel1, ref body0.transformRate.AngularVelocity, out newAngVel1); Vector3 newAngVel2;// = body1.AngVel + (newAngRot2 - angRot2) * hingeAxis; Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out newAngVel2); Vector3.Add(ref newAngVel2, ref body1.transformRate.AngularVelocity, out newAngVel2); body0.AngularVelocity = newAngVel1; body1.AngularVelocity = newAngVel2; } // the extra torque if (extraTorque != 0.0f) { Vector3 torque1;// = extraTorque * Vector3.Transform(hingeAxis, body0.Orientation); Vector3.TransformNormal(ref hingeAxis, ref body0.transform.Orientation, out torque1); Vector3.Multiply(ref torque1, extraTorque, out torque1); body0.AddWorldTorque(torque1); body1.AddWorldTorque(-torque1); } }
/// <summary> /// SegmentIntersect /// </summary> /// <param name="frac"></param> /// <param name="pos"></param> /// <param name="normal"></param> /// <param name="seg"></param> /// <returns>bool</returns> public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { bool result = Intersection.SegmentCapsuleIntersection(out frac, seg, this); if (result) { pos = seg.GetPoint(frac); normal = pos - transform.Position; normal -= Vector3.Dot(normal, transform.Orientation.Backward()) * transform.Orientation.Backward(); JiggleMath.NormalizeSafe(ref normal); } else { pos = normal = Vector3.Zero; } return(result); }
public override void UpdateController(float dt) { if (body0 == null || body1 == null) { return; } if (damping > 0.0f) { var hingeAxis = body1.AngularVelocity - body0.AngularVelocity; JiggleMath.NormalizeSafe(ref hingeAxis); Vector3.Dot(ref body0.TransformRate.AngularVelocity, ref hingeAxis, out var angRot1); Vector3.Dot(ref body1.TransformRate.AngularVelocity, ref hingeAxis, out var angRot2); var avAngRot = 0.5f * (angRot1 + angRot2); var frac = 1.0f - damping; var newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac; var newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac; Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out var newAngVel1); Vector3.Add(ref newAngVel1, ref body0.TransformRate.AngularVelocity, out newAngVel1); Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out var newAngVel2); Vector3.Add(ref newAngVel2, ref body1.TransformRate.AngularVelocity, out newAngVel2); body0.AngularVelocity = newAngVel1; body1.AngularVelocity = newAngVel2; } if (extraTorque != 0.0f) { Vector3.TransformNormal(ref hingeAxis, ref body0.Transform.Orientation, out var torque1); Vector3.Multiply(ref torque1, extraTorque, out torque1); body0.AddWorldTorque(torque1); body1.AddWorldTorque(-torque1); } }
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { bool result; result = Intersection.SegmentSphereIntersection(out frac, seg, this); if (result) { pos = seg.GetPoint(frac); normal = pos - this.transform.Position; JiggleMath.NormalizeSafe(ref normal); } else { pos = Vector3.Zero; normal = Vector3.Zero; } return(result); }
/// <summary> /// CollDetectSphereStaticMeshOverlap /// </summary> /// <param name="oldSphere"></param> /// <param name="newSphere"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public static void CollDetectSphereStaticMeshOverlap(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float sphereTolR = collTolerance + newSphere.Radius; float sphereTolR2 = sphereTolR * sphereTolR; SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); int[] potTriArray = IntStackAlloc(); int numCollPts = 0; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(newSphere, ref bb); int numTriangles = mesh.GetTrianglesIntersectingtAABox(potTriArray, MaxLocalStackTris, ref bb); // Deano : get the spheres centers in triangle mesh space Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix); Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = mesh.GetTriangle(potTriArray[iTriangle]); float distToCentre = meshTriangle.Plane.DotCoordinate(newSphereCen); // BEN-BUG-FIX: Replaced 0.0f with -sphereTolR. if (distToCentre < -sphereTolR || distToCentre > sphereTolR) { continue; } int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); float s, t; float newD2 = Distance.PointTriangleDistanceSq(out s, out t, newSphereCen, triangle); if (newD2 < sphereTolR2) { // have overlap - but actually report the old intersection float oldD2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); float dist = (float)System.Math.Sqrt((float)oldD2); float depth = oldSphere.Radius - dist; Vector3 triPointSTNorm = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref triPointSTNorm); Vector3 collisionN = (dist > float.Epsilon) ? triPointSTNorm : triangle.Normal; // since impulse get applied at the old position Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Reuse existing collPts. collPtArray[numCollPts].R0 = pt - body0Pos; collPtArray[numCollPts].R1 = pt - body1Pos; collPtArray[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPtArray, numCollPts); } FreeStackAlloc(potTriArray); FreeStackAlloc(collPtArray); }
/// <summary> /// CollDetect /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { // get the skins in the order that we're expectiing CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward()); Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward()); float radius = oldCapsule.Radius; Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; float oldSegT; float oldBoxT0, oldBoxT1, oldBoxT2; float oldDistSq = Distance.SegmentBoxDistanceSq(out oldSegT, out oldBoxT0, out oldBoxT1, out oldBoxT2, oldSeg, oldBox); float newSegT; float newBoxT0, newBoxT1, newBoxT2; float newDistSq = Distance.SegmentBoxDistanceSq(out newSegT, out newBoxT0, out newBoxT1, out newBoxT2, newSeg, newBox); if (OpenTKHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance))) { Vector3 segPos = oldSeg.GetPoint(oldSegT); Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right() + oldBoxT1 * oldBox.Orientation.Up() + oldBoxT2 * oldBox.Orientation.Backward(); float dist = (float)System.Math.Sqrt((float)oldDistSq); float depth = radius - dist; Vector3 dir; if (dist > JiggleMath.Epsilon) { dir = segPos - boxPos; JiggleMath.NormalizeSafe(ref dir); } else if ((segPos - oldBox.GetCentre()).LengthSquared > JiggleMath.Epsilon) { dir = segPos - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } else { // todo - make this not random dir = Vector3.Transform(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360)))); } unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth, BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle, float oldCentreDistToPlane, float newCentreDistToPlane, EdgesToTest edgesToTest, CornersToTest cornersToTest) { int i; var trianglePlane = triangle.Plane; N = Vector3.Zero; if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane)) { return(false); } var v0 = triangle.GetPoint(0); var v1 = triangle.GetPoint(1); var v2 = triangle.GetPoint(2); var e0 = v1 - v0; var e1 = v2 - v1; var e2 = v0 - v2; var allInside = true; var outDir0 = Vector3.Cross(e0, trianglePlane.Normal); if (Vector3.Dot(pt - v0, outDir0) > 0.0f) { allInside = false; } var outDir1 = Vector3.Cross(e1, trianglePlane.Normal); if (Vector3.Dot(pt - v1, outDir1) > 0.0f) { allInside = false; } var outDir2 = Vector3.Cross(e2, trianglePlane.Normal); if (Vector3.Dot(pt - v2, outDir2) > 0.0f) { allInside = false; } if (allInside) { N = trianglePlane.Normal; return(true); } var bestT = float.MaxValue; var Ks = newSphere.Center - oldSphere.Center; var kss = Vector3.Dot(Ks, Ks); var radius = newSphere.Radius; var radiusSq = radius * radius; for (i = 0; i < 3; ++i) { var mask = 1 << i; if (!((mask != 0) & ((int)edgesToTest != 0))) { continue; } Vector3 Ke; Vector3 vp; switch (i) { case 0: Ke = e0; vp = v0; break; case 1: Ke = e1; vp = v1; break; case 2: default: Ke = e2; vp = v2; break; } var Kg = vp - oldSphere.Center; var kee = Vector3.Dot(Ke, Ke); if (System.Math.Abs(kee) < JiggleMath.Epsilon) { continue; } var kes = Vector3.Dot(Ke, Ks); var kgs = Vector3.Dot(Kg, Ks); var keg = Vector3.Dot(Ke, Kg); var kgg = Vector3.Dot(Kg, Kg); var a = kee * kss - kes * kes; if (System.Math.Abs(a) < JiggleMath.Epsilon) { continue; } var b = 2.0f * (keg * kes - kee * kgs); var c = kee * (kgg - radiusSq) - keg * keg; var blah = b * b - 4.0f * a * c; if (blah < 0.0f) { continue; } var t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) { continue; } if (t > bestT) { continue; } var Ct = oldSphere.Center + t * Ks; var d = Vector3.Dot(Ct - vp, Ke) / kee; if (d < 0.0f || d > 1.0f) { continue; } bestT = t; pt = vp + d * Ke; N = Ct - pt; JiggleMath.NormalizeSafe(ref N); } if (bestT <= 1.0f) { return(true); } bestT = float.MaxValue; for (i = 0; i < 3; ++i) { var mask = 1 << i; if (!((mask != 0) & (cornersToTest != 0))) { continue; } Vector3 vp; switch (i) { case 0: vp = v0; break; case 1: vp = v1; break; case 2: default: vp = v2; break; } var Kg = vp - oldSphere.Center; var kgs = Vector3.Dot(Kg, Ks); var kgg = Vector3.Dot(Kg, Kg); var a = kss; if (System.Math.Abs(a) < JiggleMath.Epsilon) { continue; } var b = -2.0f * kgs; var c = kgg - radiusSq; var blah = b * b - 4.0f * a * c; if (blah < 0.0f) { continue; } var t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) { continue; } if (t > bestT) { continue; } bestT = t; var Ct = oldSphere.Center + t * Ks; N = Ct - vp; JiggleMath.NormalizeSafe(ref N); } if (bestT <= 1.0f) { return(true); } return(false); }
/// <summary> /// GetHeightAndNormal /// </summary> /// <param name="h"></param> /// <param name="normal"></param> /// <param name="point"></param> public void GetHeightAndNormal(out float h, out Vector3 normal, Vector3 point) { float x = point.X; float z = point.Z; x = OpenTKHelper.Clamp(x, xMin, xMax); z = OpenTKHelper.Clamp(z, zMin, zMax); int i0 = (int)((x - xMin) / dx); int j0 = (int)((point.Z - zMin) / dz); i0 = (int)OpenTKHelper.Clamp((int)i0, 0, mHeights.Nx - 1); j0 = (int)OpenTKHelper.Clamp((int)j0, 0, mHeights.Nz - 1); int i1 = i0 + 1; int j1 = j0 + 1; if (i1 >= (int)mHeights.Nx) { i1 = mHeights.Nx - 1; } if (j1 >= (int)mHeights.Nz) { j1 = mHeights.Nz - 1; } float iFrac = (x - (i0 * dx + xMin)) / dx; float jFrac = (z - (j0 * dz + zMin)) / dz; iFrac = OpenTKHelper.Clamp(iFrac, 0.0f, 1.0f); jFrac = OpenTKHelper.Clamp(jFrac, 0.0f, 1.0f); float h00 = mHeights[i0, j0]; float h01 = mHeights[i0, j1]; float h10 = mHeights[i1, j0]; float h11 = mHeights[i1, j1]; // All the triangles are orientated the same way. // work out the normal, then z is in the plane of this normal if ((i0 == i1) && (j0 == j1)) { normal = Vector3Extensions.Up; } else if (i0 == i1) { Vector3 right = Vector3Extensions.Right; normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), right); normal.Normalize(); } if (j0 == j1) { Vector3 backw = Vector3Extensions.Backward; normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f)); normal.Normalize(); } else if (iFrac > jFrac) { normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f)); normal.Normalize(); } else { normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz)); normal.Normalize(); } // get the plane equation // h00 is in all the triangles JiggleMath.NormalizeSafe(ref normal); Vector3 pos = new Vector3((i0 * dx + xMin), h00, (j0 * dz + zMin)); float d; Vector3.Dot(ref normal, ref pos, out d); d = -d; h = Distance.PointPlaneDistance(ref point, ref normal, d); }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; var newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; var oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward); var newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward); var radius = oldCapsule.Radius; var oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; var newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; var oldDistSq = Distance.SegmentBoxDistanceSq(out var oldSegT, out var oldBoxT0, out var oldBoxT1, out var oldBoxT2, oldSeg, oldBox); var newDistSq = Distance.SegmentBoxDistanceSq(out var newSegT, out var newBoxT0, out var newBoxT1, out var newBoxT2, newSeg, newBox); if (MathHelper.Min(oldDistSq, newDistSq) < (radius + collTolerance) * (radius + collTolerance)) { var segPos = oldSeg.GetPoint(oldSegT); var boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right + oldBoxT1 * oldBox.Orientation.Up + oldBoxT2 * oldBox.Orientation.Backward; var dist = (float)System.Math.Sqrt(oldDistSq); var depth = radius - dist; Vector3 dir; if (dist > JiggleMath.Epsilon) { dir = segPos - boxPos; JiggleMath.NormalizeSafe(ref dir); } else if ((segPos - oldBox.GetCentre()).LengthSquared() > JiggleMath.Epsilon) { dir = segPos - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } else { dir = Vector3.Transform(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(random.Next(360)))); } unsafe { var collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } }
public bool AddForcesToCar(float dt) { var force = Vector3.Zero; _lastDisplacement = Displacement; Displacement = 0.0f; var carBody = _car.Chassis.Body; var worldPos = carBody.Position + Vector3.TransformNormal(Pos, carBody.Orientation); var worldAxis = Vector3.TransformNormal(LocalAxisUp, carBody.Orientation); var wheelFwd = Vector3.TransformNormal(carBody.Orientation.Right, JiggleMath.RotationMatrix(SteerAngle, worldAxis)); var wheelUp = worldAxis; var wheelLeft = Vector3.Cross(wheelUp, wheelFwd); wheelLeft.Normalize(); wheelUp = Vector3.Cross(wheelFwd, wheelLeft); var rayLen = 2.0f * Radius + _travel; var wheelRayEnd = worldPos - Radius * worldAxis; var wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis); var collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem; var numRaysUse = System.Math.Min(_numRays, MaxNumRays); var deltaFwd = 2.0f * Radius / (numRaysUse + 1); var deltaFwdStart = deltaFwd; OnFloor = false; var bestIRay = 0; int iRay; for (iRay = 0; iRay < numRaysUse; ++iRay) { _fracs[iRay] = float.MaxValue; var distFwd = deltaFwdStart + iRay * deltaFwd - Radius; var zOffset = Radius * (1.0f - (float)System.Math.Cos(MathHelper.ToRadians(90.0f * (distFwd / Radius)))); _segments[iRay] = wheelRay; _segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp; if (collSystem.SegmentIntersect(out _fracs[iRay], out _otherSkins[iRay], out _groundPositions[iRay], out _groundNormals[iRay], _segments[iRay], _pred)) { OnFloor = true; if (_fracs[iRay] < _fracs[bestIRay]) { bestIRay = iRay; } } } if (!OnFloor) { return(false); } var groundPos = _groundPositions[bestIRay]; var frac = _fracs[bestIRay]; var otherSkin = _otherSkins[bestIRay]; var groundNormal = worldAxis; if (numRaysUse > 1) { for (iRay = 0; iRay < numRaysUse; ++iRay) { if (_fracs[iRay] <= 1.0f) { groundNormal += (1.0f - _fracs[iRay]) * (worldPos - _segments[iRay].GetEnd()); } } JiggleMath.NormalizeSafe(ref groundNormal); } else { groundNormal = _groundNormals[bestIRay]; } var worldBody = otherSkin.Owner; Displacement = rayLen * (1.0f - frac); Displacement = MathHelper.Clamp(Displacement, 0, _travel); var displacementForceMag = Displacement * _spring; displacementForceMag *= Vector3.Dot(_groundNormals[bestIRay], worldAxis); var dampingForceMag = _upSpeed * _damping; var totalForceMag = displacementForceMag + dampingForceMag; if (totalForceMag < 0.0f) { totalForceMag = 0.0f; } var extraForce = totalForceMag * worldAxis; force += extraForce; var groundUp = groundNormal; var groundLeft = Vector3.Cross(groundNormal, wheelFwd); JiggleMath.NormalizeSafe(ref groundLeft); var groundFwd = Vector3.Cross(groundLeft, groundUp); var wheelPointVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.TransformNormal(Pos, carBody.Orientation)); var rimVel = _angVel * Vector3.Cross(wheelLeft, groundPos - worldPos); wheelPointVel += rimVel; if (worldBody != null) { var worldVel = worldBody.Velocity + Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position); wheelPointVel -= worldVel; } var noslipVel = 0.2f; var slipVel = 0.4f; var slipFactor = 0.7f; float smallVel = 3; var friction = _sideFriction; var sideVel = Vector3.Dot(wheelPointVel, groundLeft); if (sideVel > slipVel || sideVel < -slipVel) { friction *= slipFactor; } else if (sideVel > noslipVel || sideVel < -noslipVel) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel); } if (sideVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(sideVel) < smallVel) { friction *= System.Math.Abs(sideVel) / smallVel; } var sideForce = -friction * totalForceMag; extraForce = sideForce * groundLeft; force += extraForce; friction = _fwdFriction; var fwdVel = Vector3.Dot(wheelPointVel, groundFwd); if (fwdVel > slipVel || fwdVel < -slipVel) { friction *= slipFactor; } else if (fwdVel > noslipVel || fwdVel < -noslipVel) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel); } if (fwdVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(fwdVel) < smallVel) { friction *= System.Math.Abs(fwdVel) / smallVel; } var fwdForce = -friction * totalForceMag; extraForce = fwdForce * groundFwd; force += extraForce; var wheelCentreVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.TransformNormal(Pos, carBody.Orientation)); _angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / Radius; _torque += -fwdForce * Radius; carBody.AddWorldForce(force, groundPos); if (worldBody != null && !worldBody.Immovable) { var maxOtherBodyAcc = 500.0f; var maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass; if (force.LengthSquared() > maxOtherBodyForce * maxOtherBodyForce) { force *= maxOtherBodyForce / force.Length(); } worldBody.AddWorldForce(-force, groundPos); } return(true); }
/// <summary> /// The AABox has a corner at the origin and size sides. /// </summary> private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts, ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq) { // The AABox faces are aligned with the world directions. Loop // over the 3 directions and do the two tests. We know that the // AABox has a corner at the origin #region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0); Vector3 edgeDir; Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir); JiggleMath.NormalizeSafe(ref edgeDir); #endregion int num = 0; for (int idir = 3; idir-- != 0;) { // skip edge/face tests if nearly parallel if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f) { continue; } int jdir = (idir + 1) % 3; int kdir = (idir + 2) % 3; for (int iface = 2; iface-- != 0;) { float offset = 0.0f; if (iface == 1) { offset = JiggleUnsafe.Get(ref sides, idir); } float dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset; float dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset; float frac = -1.0f; if (dist0 * dist1 < -JiggleMath.Epsilon) { frac = -dist0 / (dist1 - dist0); } else if (System.Math.Abs(dist0) < JiggleMath.Epsilon) { frac = 0.0f; } else if (System.Math.Abs(dist1) < JiggleMath.Epsilon) { frac = 1.0f; } if (frac >= 0.0f) { #region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1 Vector3 tmp; Vector3 pt; Vector3.Multiply(ref edgePt1, frac, out tmp); Vector3.Multiply(ref edgePt0, 1.0f - frac, out pt); Vector3.Add(ref pt, ref tmp, out pt); #endregion // check the point is within the face rectangle float ptJdir = JiggleUnsafe.Get(ref pt, jdir); float ptKdir = JiggleUnsafe.Get(ref pt, kdir); if ((ptJdir > -JiggleMath.Epsilon) && (ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon) && (ptKdir > -JiggleMath.Epsilon) && (ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon)) { // woohoo got a point #region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient); Vector3 pos; Vector3.Transform(ref pt, ref origBoxOrient, out pos); Vector3.Add(ref origBoxPos, ref pos, out pos); #endregion AddPoint(pts, ref pos, combinationDistanceSq); if (++num == 2) { return(num); } } } } } return(num); }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box; var newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box; var oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; var newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; oldBox.GetCornerPoints(out var oldPts); newBox.GetCornerPoints(out var newPts); unsafe { var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi]; { var numCollPts = 0; var collNormal = Vector3.Zero; for (var i = 0; i < 8; ++i) { var newPt = newPts[i]; newHeightmap.GetHeightAndNormal(out var newDist, out var normal, newPt); if (newDist < collTolerance) { var oldPt = oldPts[i]; var oldDist = oldHeightmap.GetHeight(oldPt); Vector3.Subtract(ref oldPt, ref body0Pos, out var pt0); Vector3.Subtract(ref oldPt, ref body1Pos, out var pt1); if (numCollPts < MaxLocalStackScpi) { collPts[numCollPts].R0 = pt0; collPts[numCollPts].R1 = pt1; collPts[numCollPts++].InitialPenetration = -oldDist; } Vector3.Add(ref collNormal, ref normal, out collNormal); } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } } }
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { var info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1) { var skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; var primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; var newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; var oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; var newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; unsafe { var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi]; { var numCollPts = 0; var averageNormal = Vector3.Zero; { oldHeightmap.GetHeightAndNormal(out var oldDist, out var normal, oldCapsule.Position); newHeightmap.GetHeightAndNormal(out var newDist, out normal, newCapsule.Position); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { var oldDepth = oldCapsule.Radius - oldDist; var worldPos = oldCapsule.Position - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackScpi) { collPts[numCollPts].R0 = worldPos - body0Pos; collPts[numCollPts].R1 = worldPos - body1Pos; collPts[numCollPts++].InitialPenetration = oldDepth; } averageNormal += normal; } } { var oldEnd = oldCapsule.GetEnd(); var newEnd = newCapsule.GetEnd(); oldHeightmap.GetHeightAndNormal(out var oldDist, out var normal, oldEnd); newHeightmap.GetHeightAndNormal(out var newDist, out normal, newEnd); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { var oldDepth = oldCapsule.Radius - oldDist; var worldPos = oldEnd - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackScpi) { collPts[numCollPts].R0 = worldPos - body0Pos; collPts[numCollPts].R1 = worldPos - body1Pos; collPts[numCollPts++].InitialPenetration = oldDepth; } averageNormal += normal; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref averageNormal); collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts); } } } }
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts, ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq) { Vector3.Subtract(ref edgePt1, ref edgePt0, out var edgeDir); JiggleMath.NormalizeSafe(ref edgeDir); var num = 0; var pt = new Vector3(); for (var idir = 3; idir-- != 0;) { if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f) { continue; } var jdir = (idir + 1) % 3; var kdir = (idir + 2) % 3; for (var iface = 2; iface-- != 0;) { var offset = 0.0f; if (iface == 1) { offset = JiggleUnsafe.Get(ref sides, idir); } var dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset; var dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset; var frac = -1.0f; if (dist0 * dist1 < -JiggleMath.Epsilon) { frac = -dist0 / (dist1 - dist0); } else if (System.Math.Abs(dist0) < JiggleMath.Epsilon) { frac = 0.0f; } else if (System.Math.Abs(dist1) < JiggleMath.Epsilon) { frac = 1.0f; } if (frac >= 0.0f) { var tempFrac = 1.0f - frac; pt.X = tempFrac * edgePt0.X + frac * edgePt1.X; pt.Y = tempFrac * edgePt0.Y + frac * edgePt1.Y; pt.Z = tempFrac * edgePt0.Z + frac * edgePt1.Z; var ptJdir = JiggleUnsafe.Get(ref pt, jdir); var ptKdir = JiggleUnsafe.Get(ref pt, kdir); if (ptJdir > -JiggleMath.Epsilon && ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon && ptKdir > -JiggleMath.Epsilon && ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon) { Vector3.TransformNormal(ref pt, ref origBoxOrient, out var pos); pos.X += origBoxPos.X; pos.Y += origBoxPos.Y; pos.Z += origBoxPos.Z; AddPoint(pts, ref pos, combinationDistanceSq); if (++num == 2) { return(num); } } } } } return(num); }
/// <summary> // TODO teting testing testing ... /// Adds the forces die to this wheel to the parent. Return value indicates if it's /// on the ground. /// </summary> /// <param name="dt"></param> public bool AddForcesToCar(float dt) { Vector3 force = Vector3.Zero; lastDisplacement = displacement; displacement = 0.0f; Body carBody = car.Chassis.Body; Vector3 worldPos = carBody.Position + Vector3.Transform(pos, carBody.Orientation); // *mPos; Vector3 worldAxis = Vector3.Transform(axisUp, carBody.Orientation); // *mAxisUp; //Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.Orientation.GetCol(0); // OpenGl has differnet row/column order for matrixes than XNA has .. Vector3 wheelFwd = Vector3.Transform(carBody.Orientation.Right, JiggleMath.RotationMatrix(steerAngle, worldAxis)); //Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.GetOrientation().GetCol(0); Vector3 wheelUp = worldAxis; Vector3 wheelLeft = Vector3.Cross(wheelUp, wheelFwd); wheelLeft.Normalize(); wheelUp = Vector3.Cross(wheelFwd, wheelLeft); // start of ray float rayLen = 2.0f * radius + travel; Vector3 wheelRayEnd = worldPos - radius * worldAxis; Segment wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis); //Assert(PhysicsSystem.CurrentPhysicsSystem); CollisionSystem collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem; ///Assert(collSystem); int numRaysUse = System.Math.Min(numRays, maxNumRays); // adjust the start position of the ray - divide the wheel into numRays+2 // rays, but don't use the first/last. float deltaFwd = (2.0f * radius) / (numRaysUse + 1); float deltaFwdStart = deltaFwd; lastOnFloor = false; int bestIRay = 0; int iRay; for (iRay = 0; iRay < numRaysUse; ++iRay) { fracs[iRay] = float.MaxValue; //SCALAR_HUGE; // work out the offset relative to the middle ray float distFwd = (deltaFwdStart + iRay * deltaFwd) - radius; //float zOffset = mRadius * (1.0f - CosDeg(90.0f * (distFwd / mRadius))); float zOffset = radius * (1.0f - (float)System.Math.Cos(MathHelper.ToRadians(90.0f * (distFwd / radius)))); segments[iRay] = wheelRay; segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp; if (collSystem.SegmentIntersect(out fracs[iRay], out otherSkins[iRay], out groundPositions[iRay], out groundNormals[iRay], segments[iRay], pred)) { lastOnFloor = true; if (fracs[iRay] < fracs[bestIRay]) { bestIRay = iRay; } } } if (!lastOnFloor) { return(false); } //Assert(bestIRay < numRays); // use the best one Vector3 groundPos = groundPositions[bestIRay]; float frac = fracs[bestIRay]; CollisionSkin otherSkin = otherSkins[bestIRay]; // const Vector3 groundNormal = (worldPos - segments[bestIRay].GetEnd()).NormaliseSafe(); // const Vector3 groundNormal = groundNormals[bestIRay]; Vector3 groundNormal = worldAxis; if (numRaysUse > 1) { for (iRay = 0; iRay < numRaysUse; ++iRay) { if (fracs[iRay] <= 1.0f) { groundNormal += (1.0f - fracs[iRay]) * (worldPos - segments[iRay].GetEnd()); } } JiggleMath.NormalizeSafe(ref groundNormal); } else { groundNormal = groundNormals[bestIRay]; } //Assert(otherSkin); Body worldBody = otherSkin.Owner; displacement = rayLen * (1.0f - frac); displacement = MathHelper.Clamp(displacement, 0, travel); float displacementForceMag = displacement * spring; // reduce force when suspension is par to ground displacementForceMag *= Vector3.Dot(groundNormals[bestIRay], worldAxis); // apply damping float dampingForceMag = upSpeed * damping; float totalForceMag = displacementForceMag + dampingForceMag; if (totalForceMag < 0.0f) { totalForceMag = 0.0f; } Vector3 extraForce = totalForceMag * worldAxis; force += extraForce; // side-slip friction and drive force. Work out wheel- and floor-relative coordinate frame Vector3 groundUp = groundNormal; Vector3 groundLeft = Vector3.Cross(groundNormal, wheelFwd); JiggleMath.NormalizeSafe(ref groundLeft); Vector3 groundFwd = Vector3.Cross(groundLeft, groundUp); Vector3 wheelPointVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.Transform(pos, carBody.Orientation));// * mPos); Vector3 rimVel = angVel * Vector3.Cross(wheelLeft, groundPos - worldPos); wheelPointVel += rimVel; // if sitting on another body then adjust for its velocity. if (worldBody != null) { Vector3 worldVel = worldBody.Velocity + Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position); wheelPointVel -= worldVel; } // sideways forces float noslipVel = 0.2f; float slipVel = 0.4f; float slipFactor = 0.7f; float smallVel = 3; float friction = sideFriction; float sideVel = Vector3.Dot(wheelPointVel, groundLeft); if ((sideVel > slipVel) || (sideVel < -slipVel)) { friction *= slipFactor; } else if ((sideVel > noslipVel) || (sideVel < -noslipVel)) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel); } if (sideVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(sideVel) < smallVel) { friction *= System.Math.Abs(sideVel) / smallVel; } float sideForce = -friction * totalForceMag; extraForce = sideForce * groundLeft; force += extraForce; // fwd/back forces friction = fwdFriction; float fwdVel = Vector3.Dot(wheelPointVel, groundFwd); if ((fwdVel > slipVel) || (fwdVel < -slipVel)) { friction *= slipFactor; } else if ((fwdVel > noslipVel) || (fwdVel < -noslipVel)) { friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel); } if (fwdVel < 0.0f) { friction *= -1.0f; } if (System.Math.Abs(fwdVel) < smallVel) { friction *= System.Math.Abs(fwdVel) / smallVel; } float fwdForce = -friction * totalForceMag; extraForce = fwdForce * groundFwd; force += extraForce; //if (!force.IsSensible()) //{ // TRACE_FILE_IF(ONCE_1) // TRACE("Bad force in car wheel\n"); // return true; //} // fwd force also spins the wheel Vector3 wheelCentreVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.Transform(pos, carBody.Orientation));// * mPos); angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / radius; torque += -fwdForce * radius; // add force to car carBody.AddWorldForce(force, groundPos); //if (float.IsNaN(force.X)) // while(true){} //System.Diagnostics.Debug.WriteLine(force.ToString()); // add force to the world if (worldBody != null && !worldBody.Immovable) { // todo get the position in the right place... // also limit the velocity that this force can produce by looking at the // mass/inertia of the other object float maxOtherBodyAcc = 500.0f; float maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass; if (force.LengthSquared() > (maxOtherBodyForce * maxOtherBodyForce)) { force *= maxOtherBodyForce / force.Length(); } worldBody.AddWorldForce(-force, groundPos); } return(true); }
private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var capsuleTolR = collTolerance + newCapsule.Radius; var capsuleTolR2 = capsuleTolR * capsuleTolR; var collNormal = Vector3.Zero; var bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddCapsule(newCapsule, ref bb); unsafe { var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi]; var potentialTriangles = stackalloc int[MaxLocalStackTris]; { { var numCollPts = 0; var numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); var capsuleStart = newCapsule.Position; var capsuleEnd = newCapsule.GetEnd(); var meshInvTransform = mesh.InverseTransformMatrix; var meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform); var meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform); for (var iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { var meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); var distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart); var distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd); if (distToStart > capsuleTolR && distToEnd > capsuleTolR || distToStart < -capsuleTolR && distToEnd < -capsuleTolR) { continue; } meshTriangle.GetVertexIndices(out var i0, out var i1, out var i2); mesh.GetVertex(i0, out var triVec0); mesh.GetVertex(i1, out var triVec1); mesh.GetVertex(i2, out var triVec2); var transformMatrix = mesh.TransformMatrix; Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0); Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1); Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2); var triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2); var seg = new Segment(capsuleStart, capsuleEnd - capsuleStart); var d2 = Distance.SegmentTriangleDistanceSq(out var tS, out var tT0, out var tT1, seg, triangle); if (d2 < capsuleTolR2) { var oldCapsuleStart = oldCapsule.Position; var oldCapsuleEnd = oldCapsule.GetEnd(); var oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart); d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle); var dist = (float)System.Math.Sqrt(d2); var depth = oldCapsule.Radius - dist; var pt = triangle.GetPoint(tT0, tT1); var collisionN = d2 > JiggleMath.Epsilon ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) : meshTriangle.Plane.Normal; if (numCollPts < MaxLocalStackScpi) { collPts[numCollPts].R0 = pt - body0Pos; collPts[numCollPts].R1 = pt - body1Pos; collPts[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } } } }
public void GetHeightAndNormal(out float h, out Vector3 normal, Vector3 point) { var x = point.X; var z = point.Z; x = MathHelper.Clamp(x, xMin, xMax); z = MathHelper.Clamp(z, zMin, zMax); var i0 = (int)((x - xMin) / dx); var j0 = (int)((point.Z - zMin) / dz); i0 = MathHelper.Clamp(i0, 0, Heights.Nx - 1); j0 = MathHelper.Clamp(j0, 0, Heights.Nz - 1); var i1 = i0 + 1; var j1 = j0 + 1; if (i1 >= Heights.Nx) { i1 = Heights.Nx - 1; } if (j1 >= Heights.Nz) { j1 = Heights.Nz - 1; } var iFrac = (x - (i0 * dx + xMin)) / dx; var jFrac = (z - (j0 * dz + zMin)) / dz; iFrac = MathHelper.Clamp(iFrac, 0.0f, 1.0f); jFrac = MathHelper.Clamp(jFrac, 0.0f, 1.0f); var h00 = Heights[i0, j0]; var h01 = Heights[i0, j1]; var h10 = Heights[i1, j0]; var h11 = Heights[i1, j1]; if (i0 == i1 && j0 == j1) { normal = Vector3.Up; } else if (i0 == i1) { var right = Vector3.Right; normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), right); normal.Normalize(); } if (j0 == j1) { var backw = Vector3.Backward; normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f)); normal.Normalize(); } else if (iFrac > jFrac) { normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f)); normal.Normalize(); } else { normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz)); normal.Normalize(); } JiggleMath.NormalizeSafe(ref normal); var pos = new Vector3(i0 * dx + xMin, h00, j0 * dz + zMin); Vector3.Dot(ref normal, ref pos, out var d); d = -d; h = Distance.PointPlaneDistance(ref point, ref normal, d); }
/// <summary> /// CollDetectSphereStaticMeshSweep /// </summary> /// <param name="oldSphere"></param> /// <param name="newSphere"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> internal static void CollDetectSphereStaticMeshSweep(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { // really use a swept test - or overlap? Vector3 delta = newSphere.Center - oldSphere.Center; if (delta.LengthSquared() < (0.25f * newSphere.Radius * newSphere.Radius)) { CollDetectSphereStaticMeshOverlap(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor); } else { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float sphereTolR = collTolerance + oldSphere.Radius; float sphereToR2 = sphereTolR * sphereTolR; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(oldSphere, ref bb); BoundingBoxHelper.AddSphere(newSphere, ref bb); // get the spheres centers in triangle mesh space Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix); Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix); unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int *potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed(int *potentialTriangles = potTriArray) { #endif int numCollPts = 0; int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { // first test the old sphere for being on the wrong side IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); float distToCentreOld = meshTriangle.Plane.DotCoordinate(oldSphereCen); if (distToCentreOld <= 0.0f) { continue; } // now test the new sphere for being clear float distToCentreNew = meshTriangle.Plane.DotCoordinate(newSphereCen); if (distToCentreNew > sphereTolR) { continue; } int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); // If the old sphere is intersecting, just use that result float s, t; float d2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); if (d2 < sphereToR2) { float dist = (float)System.Math.Sqrt(d2); float depth = oldSphere.Radius - dist; Vector3 triangleN = triangle.Normal; Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref normSafe); Vector3 collisionN = (dist > float.Epsilon) ? normSafe : triangleN; // since impulse gets applied at the old position Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Reuse existing collPts. collPts[numCollPts].R0 = pt - body0Pos; collPts[numCollPts].R1 = pt - body1Pos; collPts[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } else if (distToCentreNew < distToCentreOld) { // old sphere is not intersecting - do a sweep, but only if the sphere is moving into the // triangle Vector3 pt, N; // CHECK THIS float depth; if (Intersection.SweptSphereTriangleIntersection(out pt, out N, out depth, oldSphere, newSphere, triangle, distToCentreOld, distToCentreNew, Intersection.EdgesToTest.EdgeAll, Intersection.CornersToTest.CornerAll)) { // collision point etc must be relative to the old position because that's //where the impulses are applied float dist = (float)System.Math.Sqrt(d2); float depth2 = oldSphere.Radius - dist; Vector3 triangleN = triangle.Normal; Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref normSafe); Vector3 collisionN = (dist > JiggleMath.Epsilon) ? normSafe : triangleN; // since impulse gets applied at the old position Vector3 pt2 = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Reuse existing collPts. collPts[numCollPts].R0 = pt2 - body0Pos; collPts[numCollPts].R1 = pt2 - body1Pos; collPts[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } #if USE_STACKALLOC } } #else FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); } #endif } }
/// <summary> /// SegmentIntersect /// </summary> /// <param name="frac"></param> /// <param name="pos"></param> /// <param name="normal"></param> /// <param name="seg"></param> /// <returns>bool</returns> public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg) { /* Bug with SegmentIntersect, if the segment >= 1 square, then the code fails. * We need to start at seg.Origin, and move to seg.Origin + (dx,dz) until one is below and one is above 0) */ frac = 0; pos = Vector3.Zero; normal = Vector3.Up; Vector3 normalStart = Vector3.Zero; float heightStart = 0.0f; Vector3 normalEnd; float heightEnd; Vector3 segPos = Vector3.Zero; Vector3 segPosEnd = seg.Origin; Vector3 segEnd = seg.GetEnd(); Vector3 segDirection = seg.Delta; segDirection.Y = 0; JiggleMath.NormalizeSafe(ref segDirection); segDirection.Y = seg.Delta.Y * (segDirection.X / seg.Delta.X); // Scale the Y to be make segDirection a scaler of segDelta, but normalized in the XY plane float increment = JiggleMath.Min(dx, dz, float.MaxValue); // Move by the smaller of the two dx/dz values so we dont "jump" over a point ... this could be done better GetHeightAndNormal(out heightEnd, out normalEnd, seg.Origin); bool done = false; while (!done) { segPos = segPosEnd; normalStart = normalEnd; heightStart = heightEnd; segPosEnd += segDirection * increment; if ((((segPos.X - segEnd.X) > 0.0f) != ((segPosEnd.X - segEnd.X) > 0.0f)) || // We have moved onto the other side of the seg end in X (((segPos.Z - segEnd.Z) > 0.0f) != ((segPosEnd.Z - segEnd.Z) > 0.0f))) // We have moved onto the other side of the seg end in Z { segPosEnd = segEnd; } GetHeightAndNormal(out heightEnd, out normalEnd, segPosEnd); if ((heightStart >= 0.0f && heightEnd <= 0.0f) || // Passes down through (heightStart <= 0.0f && heightEnd >= 0.0f)) // Passes up through { done = true; //We intersect here } if (segEnd.X == segPosEnd.X && segEnd.Z == segPosEnd.Z && !done) // Checking for X,Z handles cases of segments only in Y direction { // No intersection found and we are at the end of the ray return(false); } } /*Vector3 normalStart; * float heightStart; * * GetHeightAndNormal(out heightStart, out normalStart,seg.Origin); * * if (heightStart < 0.0f) * return false; * * Vector3 normalEnd; * float heightEnd; * Vector3 end = seg.GetEnd(); * GetHeightAndNormal(out heightEnd, out normalEnd,end); * * if (heightEnd > 0.0f) * return false;*/ //Rest of calculations are % based, do not need negatives heightStart = System.Math.Abs(heightStart); heightEnd = System.Math.Abs(heightEnd); // normal is the weighted mean of these... float weightStart = 1.0f / (JiggleMath.Epsilon + heightStart); float weightEnd = 1.0f / (JiggleMath.Epsilon + heightEnd); normal = (normalStart * weightStart + normalEnd * weightEnd) / (weightStart + weightEnd); frac = heightStart / (heightStart + heightEnd + JiggleMath.Epsilon); //pos = seg.GetPoint(frac); pos = segPos + (segPosEnd - segPos) * frac; return(true); }
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth, BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle, float oldCentreDistToPlane, float newCentreDistToPlane, EdgesToTest edgesToTest, CornersToTest cornersToTest) { int i; Microsoft.Xna.Framework.Plane trianglePlane = triangle.Plane; N = Vector3.Zero; // Check against plane if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane)) { return(false); } Vector3 v0 = triangle.GetPoint(0); Vector3 v1 = triangle.GetPoint(1); Vector3 v2 = triangle.GetPoint(2); Vector3 e0 = v1 - v0; Vector3 e1 = v2 - v1; Vector3 e2 = v0 - v2; // If the point is inside the triangle, this is a hit bool allInside = true; Vector3 outDir0 = Vector3.Cross(e0, trianglePlane.Normal); if (Vector3.Dot(pt - v0, outDir0) > 0.0f) { allInside = false; } Vector3 outDir1 = Vector3.Cross(e1, trianglePlane.Normal); if (Vector3.Dot(pt - v1, outDir1) > 0.0f) { allInside = false; } Vector3 outDir2 = Vector3.Cross(e2, trianglePlane.Normal); if (Vector3.Dot(pt - v2, outDir2) > 0.0f) { allInside = false; } // Quick result? if (allInside) { N = trianglePlane.Normal; return(true); } // Now check against the edges float bestT = float.MaxValue; Vector3 Ks = newSphere.Center - oldSphere.Center; float kss = Vector3.Dot(Ks, Ks); float radius = newSphere.Radius; float radiusSq = radius * radius; for (i = 0; i < 3; ++i) { int mask = 1 << i; if (!((mask != 0) & ((int)edgesToTest != 0))) // TODO: CHECK THIS { continue; } Vector3 Ke; Vector3 vp; switch (i) { case 0: Ke = e0; vp = v0; break; case 1: Ke = e1; vp = v1; break; case 2: default: Ke = e2; vp = v2; break; } Vector3 Kg = vp - oldSphere.Center; float kee = Vector3.Dot(Ke, Ke); if (System.Math.Abs(kee) < JiggleMath.Epsilon) { continue; } float kes = Vector3.Dot(Ke, Ks); float kgs = Vector3.Dot(Kg, Ks); float keg = Vector3.Dot(Ke, Kg); float kgg = Vector3.Dot(Kg, Kg); // a * t^2 + b * t + c = 0 float a = kee * kss - (kes * kes); if (System.Math.Abs(a) < JiggleMath.Epsilon) { continue; } float b = 2.0f * (keg * kes - kee * kgs); float c = kee * (kgg - radiusSq) - keg * keg; float blah = b * b - 4.0f * a * c; if (blah < 0.0f) { continue; } // solve for t - take minimum float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) { continue; } if (t > bestT) { continue; } // now check where it hit on the edge Vector3 Ct = oldSphere.Center + t * Ks; float d = Vector3.Dot((Ct - vp), Ke) / kee; if (d < 0.0f || d > 1.0f) { continue; } // wahay - got hit. Already checked that t < bestT bestT = t; pt = vp + d * Ke; N = (Ct - pt);// .GetNormalisedSafe(); JiggleMath.NormalizeSafe(ref N); // depth is already calculated } if (bestT <= 1.0f) { return(true); } // check the corners bestT = float.MaxValue; for (i = 0; i < 3; ++i) { int mask = 1 << i; if (!((mask != 0) & (cornersToTest != 0))) // CHECK THIS { continue; } Vector3 vp; switch (i) { case 0: vp = v0; break; case 1: vp = v1; break; case 2: default: vp = v2; break; } Vector3 Kg = vp - oldSphere.Center; float kgs = Vector3.Dot(Kg, Ks); float kgg = Vector3.Dot(Kg, Kg); float a = kss; if (System.Math.Abs(a) < JiggleMath.Epsilon) { continue; } float b = -2.0f * kgs; float c = kgg - radiusSq; float blah = (b * b) - 4.0f * a * c; if (blah < 0.0f) { continue; } // solve for t - take minimum float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a); if (t < 0.0f || t > 1.0f) { continue; } if (t > bestT) { continue; } bestT = t; Vector3 Ct = oldSphere.Center + t * Ks; N = (Ct - vp);//.GetNormalisedSafe(); JiggleMath.NormalizeSafe(ref N); } if (bestT <= 1.0f) { return(true); } return(false); }
/// <summary> /// CollDetect /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule; Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule; Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); int numCollPts = 0; Vector3 averageNormal = Vector3.Zero; // the start { float oldDist, newDist; Vector3 normal; oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position); newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating. collPtArray[numCollPts].R0 = worldPos - body0Pos; collPtArray[numCollPts].R1 = worldPos - body1Pos; collPtArray[numCollPts++].InitialPenetration = oldDepth; } averageNormal += normal; } } // the end { Vector3 oldEnd = oldCapsule.GetEnd(); Vector3 newEnd = newCapsule.GetEnd(); float oldDist, newDist; Vector3 normal; oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd); newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldEnd - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating. collPtArray[numCollPts].R0 = worldPos - body0Pos; collPtArray[numCollPts].R1 = worldPos - body1Pos; collPtArray[numCollPts++].InitialPenetration = oldDepth; } averageNormal += normal; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref averageNormal); collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPtArray, numCollPts); } FreeStackAlloc(collPtArray); }
/// <summary> /// /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0); Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0); Heightmap oldHeightmap = (Heightmap)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1); Heightmap newHeightmap = (Heightmap)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1); unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; Vector3 averageNormal = Vector3.Zero; // the start { float oldDist, newDist; Vector3 normal; oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position); newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth); } averageNormal += normal; } } // the end { Vector3 oldEnd = oldCapsule.GetEnd(); Vector3 newEnd = newCapsule.GetEnd(); float oldDist, newDist; Vector3 normal; oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd); newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd); if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius) { float oldDepth = oldCapsule.Radius - oldDist; // calc the world position based on the old position(s) Vector3 worldPos = oldEnd - oldCapsule.Radius * normal; if (numCollPts < MaxLocalStackSCPI) { collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth); } averageNormal += normal; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref averageNormal); collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts); } } #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif } }
/// <summary> /// The AABox has a corner at the origin and size sides. /// </summary> /// <param name="pts"></param> /// <param name="sides"></param> /// <param name="box"></param> /// <param name="edgePt0"></param> /// <param name="edgePt1"></param> /// <param name="origBoxOrient"></param> /// <param name="origBoxPos"></param> /// <param name="combinationDistanceSq"></param> /// <returns>int</returns> private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts, ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq) { // The AABox faces are aligned with the world directions. Loop // over the 3 directions and do the two tests. We know that the // AABox has a corner at the origin #region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0); Vector3 edgeDir; Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir); JiggleMath.NormalizeSafe(ref edgeDir); #endregion int num = 0; // BEN-OPTIMISATION: Reuse the one Vector3 Vector3 pt = new Vector3(); for (int idir = 3; idir-- != 0;) { // skip edge/face tests if nearly parallel if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f) { continue; } int jdir = (idir + 1) % 3; int kdir = (idir + 2) % 3; for (int iface = 2; iface-- != 0;) { float offset = 0.0f; if (iface == 1) { offset = JiggleUnsafe.Get(ref sides, idir); } float dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset; float dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset; float frac = -1.0f; if (dist0 * dist1 < -JiggleMath.Epsilon) { frac = -dist0 / (dist1 - dist0); } else if (System.Math.Abs(dist0) < JiggleMath.Epsilon) { frac = 0.0f; } else if (System.Math.Abs(dist1) < JiggleMath.Epsilon) { frac = 1.0f; } if (frac >= 0.0f) { #region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1 // BEN-OPTIMISATION: Reusing pt and inlined. float tempFrac = 1.0f - frac; pt.X = tempFrac * edgePt0.X + frac * edgePt1.X; pt.Y = tempFrac * edgePt0.Y + frac * edgePt1.Y; pt.Z = tempFrac * edgePt0.Z + frac * edgePt1.Z; #endregion // check the point is within the face rectangle float ptJdir = JiggleUnsafe.Get(ref pt, jdir); float ptKdir = JiggleUnsafe.Get(ref pt, kdir); if ((ptJdir > -JiggleMath.Epsilon) && (ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon) && (ptKdir > -JiggleMath.Epsilon) && (ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon)) { // woohoo got a point #region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient); // BEN-OPTIMISATION: Inlined add because this function can be called very often! Vector3 pos; Vector3.TransformNormal(ref pt, ref origBoxOrient, out pos); pos.X += origBoxPos.X; pos.Y += origBoxPos.Y; pos.Z += origBoxPos.Z; #endregion AddPoint(pts, ref pos, combinationDistanceSq); if (++num == 2) { return(num); } } } } } return(num); }
/// <summary> /// CollDetectCapsuleStaticMeshOverlap /// </summary> /// <param name="oldCapsule"></param> /// <param name="newCapsule"></param> /// <param name="mesh"></param> /// <param name="info"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; float capsuleTolR = collTolerance + newCapsule.Radius; float capsuleTolR2 = capsuleTolR * capsuleTolR; Vector3 collNormal = Vector3.Zero; BoundingBox bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddCapsule(newCapsule, ref bb); unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; int *potentialTriangles = stackalloc int[MaxLocalStackTris]; { { #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) { int[] potTriArray = IntStackAlloc(); fixed(int *potentialTriangles = potTriArray) { #endif int numCollPts = 0; int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); Vector3 capsuleStart = newCapsule.Position; Vector3 capsuleEnd = newCapsule.GetEnd(); Matrix meshInvTransform = mesh.InverseTransformMatrix; Vector3 meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform); Vector3 meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform); for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); // we do the plane test using the capsule in mesh space float distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart); float distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd); // BEN-BUG-FIX: Fixed by replacing 0.0F with -capsuleTolR. if ((distToStart > capsuleTolR && distToEnd > capsuleTolR) || (distToStart < -capsuleTolR && distToEnd < -capsuleTolR)) { continue; } // we now transform the triangle into world space (we could keep leave the mesh alone // but at this point 3 vector transforms is probably not a major slow down) int i0, i1, i2; meshTriangle.GetVertexIndices(out i0, out i1, out i2); Vector3 triVec0; Vector3 triVec1; Vector3 triVec2; mesh.GetVertex(i0, out triVec0); mesh.GetVertex(i1, out triVec1); mesh.GetVertex(i2, out triVec2); // Deano move tri into world space Matrix transformMatrix = mesh.TransformMatrix; Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0); Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1); Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2); Triangle triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2); Segment seg = new Segment(capsuleStart, capsuleEnd - capsuleStart); float tS, tT0, tT1; float d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, seg, triangle); if (d2 < capsuleTolR2) { Vector3 oldCapsuleStart = oldCapsule.Position; Vector3 oldCapsuleEnd = oldCapsule.GetEnd(); Segment oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart); d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle); // report result from old position float dist = (float)System.Math.Sqrt(d2); float depth = oldCapsule.Radius - dist; Vector3 pt = triangle.GetPoint(tT0, tT1); Vector3 collisionN = (d2 > JiggleMath.Epsilon) ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) : meshTriangle.Plane.Normal; if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Reused existing collPts. collPts[numCollPts].R0 = pt - body0Pos; collPts[numCollPts].R1 = pt - body1Pos; collPts[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } #if USE_STACKALLOC } } #else } FreeStackAlloc(potTriArray); } FreeStackAlloc(collPtArray); #endif } }
/// <summary> /// Detect BoxHeightmap Collisions. /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper swept test Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box; Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box; Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap; Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap; Vector3[] oldPts, newPts; oldBox.GetCornerPoints(out oldPts); newBox.GetCornerPoints(out newPts); unsafe { #if USE_STACKALLOC SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI]; #else SmallCollPointInfo[] collPtArray = SCPIStackAlloc(); fixed(SmallCollPointInfo *collPts = collPtArray) #endif { int numCollPts = 0; Vector3 collNormal = Vector3.Zero; for (int i = 0; i < 8; ++i) { Vector3 newPt = newPts[i]; float newDist; Vector3 normal; newHeightmap.GetHeightAndNormal(out newDist, out normal, newPt); if (newDist < collTolerance) { Vector3 oldPt = oldPts[i]; float oldDist = oldHeightmap.GetHeight(oldPt); #region REFERENCE: collPts.Add(new CollPointInfo(oldPt - body0Pos, oldPt - body1Pos, -oldDist)); Vector3 pt0; Vector3 pt1; Vector3.Subtract(ref oldPt, ref body0Pos, out pt0); Vector3.Subtract(ref oldPt, ref body1Pos, out pt1); if (numCollPts < MaxLocalStackSCPI) { // BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating. collPts[numCollPts].R0 = pt0; collPts[numCollPts].R1 = pt1; collPts[numCollPts++].InitialPenetration = -oldDist; } #endregion #region REFERENCE: collNormal += normal; Vector3.Add(ref collNormal, ref normal, out collNormal); #endregion } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } #if !USE_STACKALLOC FreeStackAlloc(collPtArray); #endif } }
/// <summary> /// CollDetect /// </summary> /// <param name="infoOrig"></param> /// <param name="collTolerance"></param> /// <param name="collisionFunctor"></param> public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor) { // get the skins in the order that we're expecting CollDetectInfo info = infoOrig; if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1) { CollisionSkin skinSwap = info.Skin0; info.Skin0 = info.Skin1; info.Skin1 = skinSwap; int primSwap = info.IndexPrim0; info.IndexPrim0 = info.IndexPrim1; info.IndexPrim1 = primSwap; } Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero; Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero; // todo - proper sweep test Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere; Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere; Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box; Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box; Vector3 oldBoxPoint; Vector3 newBoxPoint; float oldDist = oldBox.GetDistanceToPoint(out oldBoxPoint, oldSphere.Position); float newDist = newBox.GetDistanceToPoint(out newBoxPoint, newSphere.Position); // normally point will be outside float oldDepth = oldSphere.Radius - oldDist; float newDepth = newSphere.Radius - newDist; if (System.Math.Max(oldDepth, newDepth) > -collTolerance) { Vector3 dir; if (oldDist < -JiggleMath.Epsilon) { dir = oldBoxPoint - oldSphere.Position - oldBoxPoint; JiggleMath.NormalizeSafe(ref dir); } else if (oldDist > JiggleMath.Epsilon) { dir = oldSphere.Position - oldBoxPoint; JiggleMath.NormalizeSafe(ref dir); } else { dir = oldSphere.Position - oldBox.GetCentre(); JiggleMath.NormalizeSafe(ref dir); } unsafe { SmallCollPointInfo collInfo = new SmallCollPointInfo(oldBoxPoint - body0Pos, oldBoxPoint - body1Pos, oldDepth); collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1); } } #endregion }
public static void CollDetectSphereStaticMeshOverlap(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor) { var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero; var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero; var sphereTolR = collTolerance + newSphere.Radius; var sphereTolR2 = sphereTolR * sphereTolR; unsafe { var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi]; var potentialTriangles = stackalloc int[MaxLocalStackTris]; { { var numCollPts = 0; var collNormal = Vector3.Zero; var bb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(newSphere, ref bb); var numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb); var newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix); var oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix); for (var iTriangle = 0; iTriangle < numTriangles; ++iTriangle) { var meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]); var distToCentre = meshTriangle.Plane.DotCoordinate(newSphereCen); if (distToCentre < -sphereTolR || distToCentre > sphereTolR) { continue; } meshTriangle.GetVertexIndices(out var i0, out var i1, out var i2); var triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2)); var newD2 = Distance.PointTriangleDistanceSq(out var s, out var t, newSphereCen, triangle); if (newD2 < sphereTolR2) { var oldD2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle); var dist = (float)System.Math.Sqrt(oldD2); var depth = oldSphere.Radius - dist; var triPointSTNorm = oldSphereCen - triangle.GetPoint(s, t); JiggleMath.NormalizeSafe(ref triPointSTNorm); var collisionN = dist > float.Epsilon ? triPointSTNorm : triangle.Normal; var pt = oldSphere.Center - oldSphere.Radius * collisionN; if (numCollPts < MaxLocalStackScpi) { collPts[numCollPts].R0 = pt - body0Pos; collPts[numCollPts].R1 = pt - body1Pos; collPts[numCollPts++].InitialPenetration = depth; } collNormal += collisionN; } } if (numCollPts > 0) { JiggleMath.NormalizeSafe(ref collNormal); collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts); } } } } }