public void Rope() { RigidBody last = null; for (int i = 0; i < RopeSize; i++) { RigidBody body = new RigidBody(new BoxShape(JVector.One)); body.Position = new JVector(i * 1.5f, 0.5f, 0); JVector jpos2 = body.Position; body.Position = jpos2; world.AddBody(body); body.Update(); if (last != null) { JVector jpos3 = last.Position; JVector dif; JVector.Subtract(ref jpos2, ref jpos3, out dif); JVector.Multiply(ref dif, 0.5f, out dif); JVector.Subtract(ref jpos2, ref dif, out dif); Constraint cons = new PointConstraint(last, body, dif); world.AddConstraint(cons); } last = body; } }
private void IntegrateCallback(object obj) { RigidBody body = obj as RigidBody; JVector temp; JVector.Multiply(ref body.linearVelocity, timestep, out temp); JVector.Add(ref temp, ref body.position, out body.position); if (!(body.isParticle)) { // pretty sure this could be more complex ... lol body.orientation += body.angularVelocity * timestep; } if ((body.Damping & RigidBody.DampingType.Linear) != 0) { JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity); } if ((body.Damping & RigidBody.DampingType.Angular) != 0) { body.angularVelocity *= currentAngularDampFactor; } body.Update(); if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts) { body.SweptExpandBoundingBox(timestep); } }
public override void Build() { AddGround(); RigidBody last = null; for (int i = 0; i < 12; i++) { var body = new RigidBody(new BoxShape(JVector.One)) { Position = new JVector((i * 1.5f) - 20, 0.5f, 0) }; var jpos2 = body.Position; Demo.World.AddBody(body); body.Update(); if (last != null) { var jpos3 = last.Position; JVector.Subtract(ref jpos2, ref jpos3, out var dif); JVector.Multiply(ref dif, 0.5f, out dif); JVector.Subtract(ref jpos2, ref dif, out dif); Constraint cons = new PointOnPoint(last, body, dif); Demo.World.AddConstraint(cons); } last = body; } }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref JVector direction, out JVector result) { result = direction; result.Normalize(); JVector.Multiply(ref result, radius, out result); }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref JVector direction, out JVector result) { JVector expandVector; JVector.Normalize(ref direction, out expandVector); JVector.Multiply(ref expandVector, sphericalExpansion, out expandVector); int minIndex = 0; float min = JVector.Dot(ref points[0], ref direction); float dot = JVector.Dot(ref points[1], ref direction); if (dot > min) { min = dot; minIndex = 1; } dot = JVector.Dot(ref points[2], ref direction); if (dot > min) { min = dot; minIndex = 2; } JVector.Add(ref points[minIndex], ref expandVector, out result); }
/// <summary> /// Multiplies a vector by a scale factor. /// </summary> /// <param name="value2">The vector to scale.</param> /// <param name="value1">The scale factor.</param> /// <returns>Returns the scaled vector.</returns> #region public static JVector operator *(Fix64 value1, JVector value2) public static JVector operator *(Fix64 value1, JVector value2) { JVector result; JVector.Multiply(ref value2, value1, out result); return(result); }
/// <summary> /// Multiply a vector with a factor. /// </summary> /// <param name="value1">The vector to multiply.</param> /// <param name="scaleFactor">The scale factor.</param> /// <returns>Returns the multiplied vector.</returns> #region public static JVector Multiply(JVector value1, Fix64 scaleFactor) public static JVector Multiply(JVector value1, Fix64 scaleFactor) { JVector result; JVector.Multiply(ref value1, scaleFactor, out result); return(result); }
private void IntegrateForces() { foreach (RigidBody body in rigidBodies) { if (!body.isStatic && body.IsActive) { JVector temp; JVector.Multiply(ref body.force, body.inverseMass * timestep, out temp); JVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity); if (!(body.isParticle)) { JVector.Multiply(ref body.torque, timestep, out temp); JVector.Transform(ref temp, ref body.invInertiaWorld, out temp); JVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity); } if (body.affectedByGravity) { JVector.Multiply(ref gravity, timestep, out temp); JVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity); } } body.force.MakeZero(); body.torque.MakeZero(); } }
public void SupportCenter(out JVector center) { center = owner.points[indices.I0].position; JVector.Add(ref center, ref owner.points[indices.I1].position, out center); JVector.Add(ref center, ref owner.points[indices.I2].position, out center); JVector.Multiply(ref center, 1.0f / 3.0f, out center); }
/// <summary> /// Applies an impulse on the center of the body. Changing /// linear velocity. /// </summary> /// <param name="impulse">Impulse direction and magnitude.</param> public void ApplyImpulse(JVector impulse) { if (this.isStatic) { throw new InvalidOperationException("Can't apply an impulse to a static body."); } JVector temp; JVector.Multiply(ref impulse, inverseMass, out temp); JVector.Add(ref linearVelocity, ref temp, out linearVelocity); }
private void IntegrateCallback(object obj) { RigidBody body = obj as RigidBody; JVector temp; JVector.Multiply(ref body.linearVelocity, timestep, out temp); JVector.Add(ref temp, ref body.position, out body.position); if (!(body.isParticle)) { //exponential map JVector axis; double angle = body.angularVelocity.Length(); if (angle < 0.001f) { // use Taylor's expansions of sync function // axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle); JVector.Multiply(ref body.angularVelocity, (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle), out axis); } else { // sync(fAngle) = sin(c*fAngle)/t JVector.Multiply(ref body.angularVelocity, ((double)Math.Sin(0.5f * angle * timestep) / angle), out axis); } JQuaternion dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (double)Math.Cos(angle * timestep * 0.5f)); JQuaternion ornA; JQuaternion.CreateFromMatrix(ref body.orientation, out ornA); JQuaternion.Multiply(ref dorn, ref ornA, out dorn); dorn.Normalize(); JMatrix.CreateFromQuaternion(ref dorn, out body.orientation); } if ((body.Damping & RigidBody.DampingType.Linear) != 0) { JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity); } if ((body.Damping & RigidBody.DampingType.Angular) != 0) { JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity); } body.Update(); if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts) { body.SweptExpandBoundingBox(timestep); } }
/// <summary> /// Applies an impulse on the specific position. Changing linear /// and angular velocity. /// </summary> /// <param name="impulse">Impulse direction and magnitude.</param> /// <param name="relativePosition">The position where the impulse gets applied /// in Body coordinate frame.</param> public void ApplyImpulse(JVector impulse, JVector relativePosition) { if (this.isStatic) { throw new InvalidOperationException("Can't apply an impulse to a static body."); } JVector temp; JVector.Multiply(ref impulse, inverseMass, out temp); JVector.Add(ref linearVelocity, ref temp, out linearVelocity); JVector.Cross(ref relativePosition, ref impulse, out temp); JVector.Transform(ref temp, ref invInertiaWorld, out temp); JVector.Add(ref angularVelocity, ref temp, out angularVelocity); }
private void IntegrateCallback(object obj) { var body = obj as RigidBody; JVector temp; JVector.Multiply(ref body.linearVelocity, timestep, out temp); JVector.Add(ref temp, ref body.position, out body.position); if (!(body.isParticle)) { JVector axis; float angle = body.angularVelocity.Length(); if (angle < 0.001f) { JVector.Multiply(ref body.angularVelocity, (0.5f*timestep - (timestep*timestep*timestep)*(0.020833333333f)*angle*angle), out axis); } else { JVector.Multiply(ref body.angularVelocity, ((float) Math.Sin(0.5f*angle*timestep)/angle), out axis); } var dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (float) Math.Cos(angle*timestep*0.5f)); JQuaternion ornA; JQuaternion.CreateFromMatrix(ref body.orientation, out ornA); JQuaternion.Multiply(ref dorn, ref ornA, out dorn); dorn.Normalize(); JMatrix.CreateFromQuaternion(ref dorn, out body.orientation); } if ((body.Damping & RigidBody.DampingType.Linear) != 0) JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity); if ((body.Damping & RigidBody.DampingType.Angular) != 0) JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity); body.Update(); if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts) body.SweptExpandBoundingBox(timestep); }
/// <summary> /// Create a bounding box appropriate for a child, based on a parents AABox /// </summary> /// <param name="aabb"></param> /// <param name="child"></param> /// <param name="result"></param> #region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result) private void CreateAABox(ref JBBox aabb, EChild child, out JBBox result) { JVector dims; JVector.Subtract(ref aabb.Max, ref aabb.Min, out dims); JVector.Multiply(ref dims, 0.5f, out dims); JVector offset = JVector.Zero; switch (child) { case EChild.PPP: offset = new JVector(1, 1, 1); break; case EChild.PPM: offset = new JVector(1, 1, 0); break; case EChild.PMP: offset = new JVector(1, 0, 1); break; case EChild.PMM: offset = new JVector(1, 0, 0); break; case EChild.MPP: offset = new JVector(0, 1, 1); break; case EChild.MPM: offset = new JVector(0, 1, 0); break; case EChild.MMP: offset = new JVector(0, 0, 1); break; case EChild.MMM: offset = new JVector(0, 0, 0); break; default: System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child"); break; } result = new JBBox(); result.Min = new JVector(offset.X * dims.X, offset.Y * dims.Y, offset.Z * dims.Z); JVector.Add(ref result.Min, ref aabb.Min, out result.Min); JVector.Add(ref result.Min, ref dims, out result.Max); // expand it just a tiny bit just to be safe! double extra = 0.00001f; JVector temp; JVector.Multiply(ref dims, extra, out temp); JVector.Subtract(ref result.Min, ref temp, out result.Min); JVector.Add(ref result.Max, ref temp, out result.Max); }
/// <summary> /// Iteratively solve this constraint. /// </summary> public override void Iterate() { if (skipConstraint) { return; } float jv = JVector.Dot(ref body1.linearVelocity, ref jacobian[0]); jv += JVector.Dot(ref body2.linearVelocity, ref jacobian[1]); float softnessScalar = accumulatedImpulse * softnessOverDt; float lambda = -effectiveMass * (jv + bias + softnessScalar); if (behavior == DistanceBehavior.LimitMinimumDistance) { float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = JMath.Max(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; } else if (behavior == DistanceBehavior.LimitMaximumDistance) { float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = JMath.Min(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; } else { accumulatedImpulse += lambda; } JVector temp; if (!body1.isStatic) { JVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp); JVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity); } if (!body2.isStatic) { JVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp); JVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity); } }
/// <summary> /// Sets the current shape. First <see cref="Prepare"/> has to be called. /// After SetCurrentShape the shape immitates another shape. /// </summary> /// <param name="index"></param> public override void SetCurrentShape(int index) { vecs[0] = octree.GetVertex(octree.tris[potentialTriangles[index]].I0); vecs[1] = octree.GetVertex(octree.tris[potentialTriangles[index]].I1); vecs[2] = octree.GetVertex(octree.tris[potentialTriangles[index]].I2); JVector sum = vecs[0]; JVector.Add(ref sum, ref vecs[1], out sum); JVector.Add(ref sum, ref vecs[2], out sum); JVector.Multiply(ref sum, 1.0f / 3.0f, out sum); geomCen = sum; JVector.Subtract(ref vecs[1], ref vecs[0], out sum); JVector.Subtract(ref vecs[2], ref vecs[0], out normal); JVector.Cross(ref sum, ref normal, out normal); }
private void FindSupportPoints(RigidBody body1, RigidBody body2, Shape shape1, Shape shape2, ref JVector point, ref JVector normal, out JVector point1, out JVector point2) { JVector mn; JVector.Negate(ref normal, out mn); JVector sA; SupportMapping(body1, shape1, ref mn, out sA); JVector sB; SupportMapping(body2, shape2, ref normal, out sB); JVector.Subtract(ref sA, ref point, out sA); JVector.Subtract(ref sB, ref point, out sB); float dot1 = JVector.Dot(ref sA, ref normal); float dot2 = JVector.Dot(ref sB, ref normal); JVector.Multiply(ref normal, dot1, out sA); JVector.Multiply(ref normal, dot2, out sB); JVector.Add(ref point, ref sA, out point1); JVector.Add(ref point, ref sB, out point2); }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref JVector direction, out JVector result) { // this Sqrt is bad! float r = (float)Math.Sqrt(direction.X * direction.X); if (Math.Abs(direction.Y) > 0.0f) { JVector dir; JVector.Normalize(ref direction, out dir); JVector.Multiply(ref dir, radius, out result); result.Y += Math.Sign(direction.Y) * 0.5f * length; } else if (r > 0.0f) { result.X = direction.X / r * radius; result.Y = 0.0f; } else { result.X = 0.0f; result.Y = 0.0f; } }
public override void Update(uint deltaMs) { var entities = OwnerApp.EntityManager.WithComponent <MovableComponent>().ToArray(); foreach (var entity in entities) { var ridgidBodyComponent = OwnerApp.EntityManager.GetComponent <RigidBodyComponent>(entity); var movableComponent = OwnerApp.EntityManager.GetComponent <MovableComponent>(entity); var velocity = JVector.Multiply(CalcVelocity(movableComponent.Speed), deltaMs / 16.6f); if (velocity != JVector.Zero && !(ridgidBodyComponent.Body is null)) { ridgidBodyComponent.Body.ApplyImpulse(velocity); } if (Keyboard.GetState().IsKeyDown(Key.Space)) // TODO && !ridgidBodyComponent.Body.IsActive) { ridgidBodyComponent.Body.ApplyImpulse(new JVector(0.0f, movableComponent.JumpSpeed, 0.0f)); } } }
/// <summary> /// Sets the current shape. First <see cref="Prepare"/> has to be called. /// After SetCurrentShape the shape immitates another shape. /// </summary> /// <param name="index"></param> public override void SetCurrentShape(int index) { bool leftTriangle = false; if (index >= numX * numZ) { leftTriangle = true; index -= numX * numZ; } int quadIndexX = index % numX; int quadIndexZ = index / numX; // each quad has two triangles, called 'leftTriangle' and !'leftTriangle' if (leftTriangle) { points[0].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ); points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ); points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ); } else { points[0].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ); points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ); points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ); } JVector sum = points[0]; JVector.Add(ref sum, ref points[1], out sum); JVector.Add(ref sum, ref points[2], out sum); JVector.Multiply(ref sum, 1.0f / 3.0f, out sum); geomCen = sum; JVector.Subtract(ref points[1], ref points[0], out sum); JVector.Subtract(ref points[2], ref points[0], out normal); JVector.Cross(ref sum, ref normal, out normal); }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref JVector direction, out JVector result) { double r = (double)Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z); if (Math.Abs(direction.Y) > 0.0f) { JVector dir; JVector.Normalize(ref direction, out dir); JVector.Multiply(ref dir, radius, out result); result.Y += Math.Sign(direction.Y) * 0.5f * length; } else if (r > 0.0f) { result.X = direction.X / r * radius; result.Y = 0.0f; result.Z = direction.Z / r * radius; } else { result.X = 0.0f; result.Y = 0.0f; result.Z = 0.0f; } }
public override void SetCurrentShape(int index) { bool leftTriangle = false; if (index >= numX * numZ) { leftTriangle = true; index -= numX * numZ; } int quadIndexX = index % numX; int quadIndexZ = index / numX; if (leftTriangle) { points[0] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ); points[1] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ); points[2] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ); } else { points[0] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ); points[1] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ); points[2] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ); } var sum = points[0]; JVector.Add(sum, points[1], out sum); JVector.Add(sum, points[2], out sum); JVector.Multiply(sum, 1.0f / 3.0f, out sum); geomCen = sum; JVector.Subtract(points[1], points[0], out sum); JVector.Subtract(points[2], points[0], out normal); JVector.Cross(sum, normal, out normal); }
// see: btSubSimplexConvexCast.cpp /// <summary> /// Checks if a ray definied through it's origin and direction collides /// with a shape. /// </summary> /// <param name="support">The supportmap implementation representing the shape.</param> /// <param name="orientation">The orientation of the shape.</param> /// <param name="invOrientation">The inverse orientation of the shape.</param> /// <param name="position">The position of the shape.</param> /// <param name="origin">The origin of the ray.</param> /// <param name="direction">The direction of the ray.</param> /// <param name="fraction">The fraction which gives information where at the /// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param> /// <param name="normal">The normal from the ray collision.</param> /// <returns>Returns true if the ray hit the shape, false otherwise.</returns> public static bool Raycast(ISupportMappable support, ref JMatrix orientation, ref JMatrix invOrientation, ref JVector position, ref JVector origin, ref JVector direction, out float fraction, out JVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); normal = JVector.Zero; fraction = float.MaxValue; float lambda = 0.0f; JVector r = direction; JVector x = origin; JVector w, p, v; JVector arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint); JVector.Subtract(ref x, ref arbitraryPoint, out v); int maxIter = MaxIterations; float distSq = v.LengthSquared(); float epsilon = 0.000001f; float VdotR; while ((distSq > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); JVector.Subtract(ref x, ref p, out w); float VdotW = JVector.Dot(ref v, ref w); if (VdotW > 0.0f) { VdotR = JVector.Dot(ref v, ref r); if (VdotR >= -JMath.Epsilon) { simplexSolverPool.GiveBack(simplexSolver); return(false); } else { lambda = lambda - VdotW / VdotR; JVector.Multiply(ref r, lambda, out x); JVector.Add(ref origin, ref x, out x); JVector.Subtract(ref x, ref p, out w); normal = v; } } if (!simplexSolver.InSimplex(w)) { simplexSolver.AddVertex(w, x, p); } if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); } else { distSq = 0.0f; } } #region Retrieving hitPoint // Giving back the fraction like this *should* work // but is inaccurate against large objects: // fraction = lambda; JVector p1, p2; simplexSolver.ComputePoints(out p1, out p2); p2 = p2 - origin; fraction = p2.Length() / direction.Length(); #endregion if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) { normal.Normalize(); } simplexSolverPool.GiveBack(simplexSolver); return(true); }
/// <summary> /// Checks two shapes for collisions. /// </summary> /// <param name="support1">The SupportMappable implementation of the first shape to test.</param> /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param> /// <param name="orientation1">The orientation of the first shape.</param> /// <param name="orientation2">The orientation of the second shape.</param> /// <param name="position1">The position of the first shape.</param> /// <param name="position2">The position of the second shape</param> /// <param name="point">The pointin world coordinates, where collision occur.</param> /// <param name="normal">The normal pointing from body2 to body1.</param> /// <param name="penetration">Estimated penetration depth of the collision.</param> /// <returns>Returns true if there is a collision, false otherwise.</returns> public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, ref JMatrix orientation2, ref JVector position1, ref JVector position2, out JVector point, out JVector normal, out float penetration) { // Used variables JVector temp1, temp2; JVector v01, v02, v0; JVector v11, v12, v1; JVector v21, v22, v2; JVector v31, v32, v3; JVector v41, v42, v4; JVector mn; // Initialization of the output point = normal = JVector.Zero; penetration = 0.0f; //JVector right = JVector.Right; // Get the center of shape1 in world coordinates -> v01 support1.SupportCenter(out v01); JVector.Transform(ref v01, ref orientation1, out v01); JVector.Add(ref position1, ref v01, out v01); // Get the center of shape2 in world coordinates -> v02 support2.SupportCenter(out v02); JVector.Transform(ref v02, ref orientation2, out v02); JVector.Add(ref position2, ref v02, out v02); // v0 is the center of the minkowski difference JVector.Subtract(ref v02, ref v01, out v0); // Avoid case where centers overlap -- any direction is fine in this case if (v0.IsNearlyZero()) { v0 = new JVector(0.00001f, 0, 0); } // v1 = support in direction of origin mn = v0; JVector.Negate(ref v0, out normal); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12); JVector.Subtract(ref v12, ref v11, out v1); if (JVector.Dot(ref v1, ref normal) <= 0.0f) { return(false); } // v2 = support perpendicular to v1,v0 JVector.Cross(ref v1, ref v0, out normal); if (normal.IsNearlyZero()) { JVector.Subtract(ref v1, ref v0, out normal); normal.Normalize(); point = v11; JVector.Add(ref point, ref v12, out point); JVector.Multiply(ref point, 0.5f, out point); JVector.Subtract(ref v12, ref v11, out temp1); penetration = JVector.Dot(ref temp1, ref normal); //point = v11; //point2 = v12; return(true); } JVector.Negate(ref normal, out mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22); JVector.Subtract(ref v22, ref v21, out v2); if (JVector.Dot(ref v2, ref normal) <= 0.0f) { return(false); } // Determine whether origin is on + or - side of plane (v1,v0,v2) JVector.Subtract(ref v1, ref v0, out temp1); JVector.Subtract(ref v2, ref v0, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); float dist = JVector.Dot(ref normal, ref v0); // If the origin is on the - side of the plane, reverse the direction of the plane if (dist > 0.0f) { JVector.Swap(ref v1, ref v2); JVector.Swap(ref v11, ref v21); JVector.Swap(ref v12, ref v22); JVector.Negate(ref normal, out normal); } int phase2 = 0; int phase1 = 0; bool hit = false; // Phase One: Identify a portal while (true) { if (phase1 > MaximumIterations) { return(false); } phase1++; // Obtain the support point in a direction perpendicular to the existing plane // Note: This point is guaranteed to lie off the plane JVector.Negate(ref normal, out mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32); JVector.Subtract(ref v32, ref v31, out v3); if (JVector.Dot(ref v3, ref normal) <= 0.0f) { return(false); } // If origin is outside (v1,v0,v3), then eliminate v2 and loop JVector.Cross(ref v1, ref v3, out temp1); if (JVector.Dot(ref temp1, ref v0) < 0.0f) { v2 = v3; v21 = v31; v22 = v32; JVector.Subtract(ref v1, ref v0, out temp1); JVector.Subtract(ref v3, ref v0, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); continue; } // If origin is outside (v3,v0,v2), then eliminate v1 and loop JVector.Cross(ref v3, ref v2, out temp1); if (JVector.Dot(ref temp1, ref v0) < 0.0f) { v1 = v3; v11 = v31; v12 = v32; JVector.Subtract(ref v3, ref v0, out temp1); JVector.Subtract(ref v2, ref v0, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); continue; } // Phase Two: Refine the portal // We are now inside of a wedge... while (true) { phase2++; // Compute normal of the wedge face JVector.Subtract(ref v2, ref v1, out temp1); JVector.Subtract(ref v3, ref v1, out temp2); JVector.Cross(ref temp1, ref temp2, out normal); // Can this happen??? Can it be handled more cleanly? if (normal.IsNearlyZero()) { return(true); } normal.Normalize(); // Compute distance from origin to wedge face float d = JVector.Dot(ref normal, ref v1); // If the origin is inside the wedge, we have a hit if (d >= 0 && !hit) { // HIT!!! hit = true; } // Find the support point in the direction of the wedge face JVector.Negate(ref normal, out mn); SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41); SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42); JVector.Subtract(ref v42, ref v41, out v4); JVector.Subtract(ref v4, ref v3, out temp1); float delta = JVector.Dot(ref temp1, ref normal); penetration = JVector.Dot(ref v4, ref normal); // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations) { if (hit) { JVector.Cross(ref v1, ref v2, out temp1); float b0 = JVector.Dot(ref temp1, ref v3); JVector.Cross(ref v3, ref v2, out temp1); float b1 = JVector.Dot(ref temp1, ref v0); JVector.Cross(ref v0, ref v1, out temp1); float b2 = JVector.Dot(ref temp1, ref v3); JVector.Cross(ref v2, ref v1, out temp1); float b3 = JVector.Dot(ref temp1, ref v0); float sum = b0 + b1 + b2 + b3; if (sum <= 0) { b0 = 0; JVector.Cross(ref v2, ref v3, out temp1); b1 = JVector.Dot(ref temp1, ref normal); JVector.Cross(ref v3, ref v1, out temp1); b2 = JVector.Dot(ref temp1, ref normal); JVector.Cross(ref v1, ref v2, out temp1); b3 = JVector.Dot(ref temp1, ref normal); sum = b1 + b2 + b3; } float inv = 1.0f / sum; JVector.Multiply(ref v01, b0, out point); JVector.Multiply(ref v11, b1, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v21, b2, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v31, b3, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v02, b0, out temp2); JVector.Add(ref temp2, ref point, out point); JVector.Multiply(ref v12, b1, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v22, b2, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref v32, b3, out temp1); JVector.Add(ref point, ref temp1, out point); JVector.Multiply(ref point, inv * 0.5f, out point); } // Compute the barycentric coordinates of the origin return(hit); } ////// Compute the tetrahedron dividing face (v4,v0,v1) //JVector.Cross(ref v4, ref v1, out temp1); //float d1 = JVector.Dot(ref temp1, ref v0); ////// Compute the tetrahedron dividing face (v4,v0,v2) //JVector.Cross(ref v4, ref v2, out temp1); //float d2 = JVector.Dot(ref temp1, ref v0); // Compute the tetrahedron dividing face (v4,v0,v3) JVector.Cross(ref v4, ref v0, out temp1); float dot = JVector.Dot(ref temp1, ref v1); if (dot >= 0.0f) { dot = JVector.Dot(ref temp1, ref v2); if (dot >= 0.0f) { // Inside d1 & inside d2 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } else { // Inside d1 & outside d2 ==> eliminate v3 v3 = v4; v31 = v41; v32 = v42; } } else { dot = JVector.Dot(ref temp1, ref v3); if (dot >= 0.0f) { // Outside d1 & inside d3 ==> eliminate v2 v2 = v4; v21 = v41; v22 = v42; } else { // Outside d1 & outside d3 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } } } } }