/// <summary>
/// The cross product of two vectors.
/// </summary>
/// <param name="vector1">The first vector.</param>
/// <param name="vector2">The second vector.</param>
/// <returns>The cross product of both vectors.</returns>
#region public static JVector Cross(JVector vector1, JVector vector2)
public static JVector Cross(JVector vector1, JVector vector2)
{
JVector result;
JVector.Cross(ref vector1, ref vector2, out result);
return(result);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(JVector force, JVector pos)
{
JVector.Add(ref this.force, ref force, out this.force);
JVector.Subtract(ref pos, ref this.position, out pos);
JVector.Cross(ref pos, ref force, out pos);
JVector.Add(ref pos, ref this.torque, out this.torque);
}
public void GetNormal(out JVector normal)
{
JVector sum;
JVector.Subtract(ref owner.points[indices.I1].position, ref owner.points[indices.I0].position, out sum);
JVector.Subtract(ref owner.points[indices.I2].position, ref owner.points[indices.I0].position, out normal);
JVector.Cross(ref sum, ref normal, out normal);
}
// sort cached points so most isolated points come first
private int SortCachedPoints(ref JVector realRelPos1, float pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
float maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res0 = cross.LengthSquared();
}
if (maxPenetrationIndex != 1)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res1 = cross.LengthSquared();
}
if (maxPenetrationIndex != 2)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[3].relativePos1, ref contactList[1].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res2 = cross.LengthSquared();
}
if (maxPenetrationIndex != 3)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[2].relativePos1, ref contactList[1].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res3 = cross.LengthSquared();
}
int biggestarea = MaxAxis(res0, res1, res2, res3);
return(biggestarea);
}
public static JVector ComputeNormal(JVector p0, JVector p1, JVector p2, WindingTypes winding,
bool shouldNormalize = true)
{
var v0 = JVector.Subtract(p1, p0);
var v1 = JVector.Subtract(p2, p0);
// This calculation is the same as the one used in a constructor below, but due to using JVector vs. vec3,
// it's easier to just duplicate the code.
var v = JVector.Cross(v0, v1) * (winding == WindingTypes.Clockwise ? 1 : -1);
return(shouldNormalize ? JVector.Normalize(v) : v);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(JVector force, JVector pos)
{
JVector.Add(ref this.force, ref force, out this.force);
JVector.Subtract(ref pos, ref this.position, out pos);
// 3d version
//JVector.Cross(ref pos, ref force, out pos);
//JVector.Add(ref pos, ref this.torque, out this.torque);
//body->t += cpvcross(r, force);
this.torque += JVector.Cross(pos, force);
}
public LimitedHingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis, float hingeFwdAngle, float hingeBckAngle) : base(world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
var pos1 = position;
JVector.Add(pos1, hingeAxis, out pos1);
var pos2 = position;
JVector.Subtract(pos2, hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
hingeAxis = JVector.Normalize(hingeAxis);
var perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
var sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir = JVector.Normalize(perpDir);
float len = 10.0f * 3;
var hingeRelAnchorPos0 = perpDir * len;
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
var hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
var hingePos = body1.Position;
var relPos0c = hingePos + hingeRelAnchorPos0;
var relPos1c = hingePos + hingeRelAnchorPos1;
DistanceConstraint = new PointPointDistance(body1, body2, relPos0c, relPos1c)
{
Distance = allowedDistance,
Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance
};
}
/// Test if point p and d lie on opposite sides of plane through abc
public int PointOutsideOfPlane(JVector p, JVector a, JVector b, JVector c, JVector d)
{
JVector normal = JVector.Cross(b - a, c - a);
float signp = JVector.Dot(p - a, normal); // [AP AB AC]
float signd = JVector.Dot(d - a, normal); // [AD AB AC]
//if (CatchDegenerateTetrahedron)
if (signd * signd < (1e-4f * 1e-4f))
{
return(-1);
}
// Points on opposite sides if expression signs are opposite
return(signp * signd < 0f ? 1 : 0);
}
/// <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);
}
/// <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);
}
// This is primarily used for tracking relative rotation on moving platforms.
public static float ComputeYaw(this JMatrix matrix)
{
// See https://stackoverflow.com/a/4341489/7281613.
JVector t = JVector.Transform(JVector.Left, matrix);
JVector f = t - JVector.Dot(t, JVector.Up) * JVector.Up;
f.Normalize();
float angle = (float)Math.Acos(JVector.Dot(JVector.Left, f));
float d = JVector.Dot(JVector.Up, JVector.Cross(JVector.Left, f));
if (d < 0)
{
angle = Constants.TwoPi - angle;
}
return(angle);
}
public static bool OriginInTriangle(JVector a, JVector b, JVector c)
{
float pab = JVector.Cross(JVector.Negate(a), b - a);
float pbc = JVector.Cross(JVector.Negate(b), c - b);
if (!SameSign(pab, pbc))
{
return(false);
}
float pca = JVector.Cross(JVector.Negate(c), a - c);
if (!SameSign(pab, pca))
{
return(false);
}
return(true);
}
/// <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);
}
//public JConvexHull ConvexHull = new JConvexHull();
public ConvexHullPrimitive(GraphicsDevice device, List <JVector> pointCloud, List <Tuple <int, int, int> > indices)
{
//JConvexHull.Build(pointCloud, JConvexHull.Approximation.Level5);
int counter = 0;
foreach (Tuple <int, int, int> face in indices)
{
JVector a = pointCloud[face.Item2] - pointCloud[face.Item1];
JVector b = pointCloud[face.Item3] - pointCloud[face.Item1];
JVector normal = JVector.Cross(a, b);
AddVertex(Conversion.ToXnaVector(pointCloud[face.Item1]), Conversion.ToXnaVector(normal));
AddVertex(Conversion.ToXnaVector(pointCloud[face.Item2]), Conversion.ToXnaVector(normal));
AddVertex(Conversion.ToXnaVector(pointCloud[face.Item3]), Conversion.ToXnaVector(normal));
AddIndex(counter + 0);
AddIndex(counter + 1);
AddIndex(counter + 2);
counter += 3;
}
//foreach (JConvexHull.Face face in ConvexHull.HullFaces) {
// AddVertex(Conversion.ToXnaVector(pointCloud[face.VertexC]), Conversion.ToXnaVector(face.Normal));
// AddVertex(Conversion.ToXnaVector(pointCloud[face.VertexB]), Conversion.ToXnaVector(face.Normal));
// AddVertex(Conversion.ToXnaVector(pointCloud[face.VertexA]), Conversion.ToXnaVector(face.Normal));
// AddIndex(counter + 0);
// AddIndex(counter + 1);
// AddIndex(counter + 2);
// counter += 3;
//}
InitializePrimitive(device);
}
public void DrawTriangle(JVector pos1, JVector pos2, JVector pos3)
{
JVector n = JVector.Cross(pos2 - pos1, pos3 - pos1);
n.Normalize();
Vector3 xn = new Vector3(n.X, n.Y, n.Z);
VertexPositionColorNormal[] tri = new VertexPositionColorNormal[3] {
new VertexPositionColorNormal(new Vector3(pos1.X, pos1.Y, pos1.Z), Color.Green, xn),
new VertexPositionColorNormal(new Vector3(pos3.X, pos3.Y, pos3.Z), Color.Green, xn),
new VertexPositionColorNormal(new Vector3(pos2.X, pos2.Y, pos2.Z), Color.Green, xn),
};
effect.Parameters["World"].SetValue(Matrix.Identity);
effect.Parameters["ViewProj"].SetValue(Camera.CurrentCamera.View * Camera.CurrentCamera.Projection);
effect.CurrentTechnique = effect.Techniques["VBO"];
foreach (EffectPass p in effect.CurrentTechnique.Passes)
{
p.Apply();
device.DrawUserPrimitives(PrimitiveType.TriangleList, tri, 0, 1);
}
}
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);
}
/// <summary>
/// Initializes a new instance of the HingeJoint class.
/// </summary>
/// <param name="world">The world class where the constraints get added to.</param>
/// <param name="body1">The first body connected to the second one.</param>
/// <param name="body2">The second body connected to the first one.</param>
/// <param name="position">The position in world space where both bodies get connected.</param>
/// <param name="hingeAxis">The axis if the hinge.</param>
public LimitedHingeJoint(JitterWorld world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis,
float hingeFwdAngle, float hingeBckAngle)
: base(world)
{
// Create the hinge first, two point constraints
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
JVector pos1 = position; JVector.Add(ref pos1, ref hingeAxis, out pos1);
JVector pos2 = position; JVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
// Now the limit, one max distance constraint
hingeAxis.Normalize();
// choose a direction that is perpendicular to the hinge
JVector perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
// now make it perpendicular to the hinge
JVector sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * 3;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
JVector hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
JVector hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
JVector hingePos = body1.Position;
JVector relPos0c = hingePos + hingeRelAnchorPos0;
JVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance(body1, body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
}
public void GetNormal(out JVector normal)
{
JVector.Subtract(Owner.points[indices.I1].position, Owner.points[indices.I0].position, out var sum);
JVector.Subtract(Owner.points[indices.I2].position, Owner.points[indices.I0].position, out normal);
JVector.Cross(sum, normal, out normal);
}
/// <summary>
/// Calculates the cross product of two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>Returns the cross product of both.</returns>
#region public static JVector operator %(JVector value1, JVector value2)
public static JVector operator %(JVector value1, JVector value2)
{
JVector result; JVector.Cross(ref value1, ref value2, out result);
return(result);
}
/// <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;
}
}
}
}
}
public void PreStep(float timeStep)
{
float vel = car.LinearVelocity.Length();
SideFriction = 2.5f - JMath.Clamp(vel / 20.0f, 0.0f, 1.4f);
ForwardFriction = 5.5f - JMath.Clamp(vel / 20.0f, 0.0f, 5.4f);
JVector force = JVector.Zero;
JVector worldAxis = JVector.Transform(JVector.Up, car.Orientation);
JVector worldPos = car.Position + JVector.Transform(Position, car.Orientation);
JVector forward = new JVector(-car.Orientation.M31, -car.Orientation.M32, -car.Orientation.M33);
JVector wheelFwd = JVector.Transform(forward, JMatrix.CreateFromAxisAngle(JVector.Up, SteerAngle / 360 * 2 * JMath.Pi));
JVector wheelLeft = JVector.Cross(JVector.Up, wheelFwd); wheelLeft.Normalize();
JVector wheelUp = JVector.Cross(wheelFwd, wheelLeft);
float rayLen = 2.0f * Radius + WheelTravel;
JVector wheelRayStart = worldPos;
JVector wheelDelta = -Radius * worldAxis;
JVector wheelRayEnd = worldPos + wheelDelta;
float deltaFwd = (2.0f * Radius) / (NumberOfRays + 1);
float deltaFwdStart = deltaFwd;
lastDisplacement = displacement;
displacement = 0.0f;
lastOnFloor = false;
JVector rayOrigin = car.Position + JVector.Transform(Position, car.Orientation);
JVector groundNormal = JVector.Zero;
JVector groundPos = JVector.Zero;
float deepestFrac = float.MaxValue;
RigidBody worldBody = null;
for (int i = 0; i < NumberOfRays; i++)
{
float distFwd = (deltaFwdStart + i * deltaFwd) - Radius;
float zOffset = Radius * (1.0f - (float)Math.Cos(Math.PI / 4 * (distFwd / Radius)));
JVector newOrigin = wheelRayStart + distFwd * wheelFwd + zOffset * wheelUp;
RigidBody body; JVector normal; float frac;
bool result = world.CollisionSystem.Raycast(newOrigin, wheelDelta,
raycast, out body, out normal, out frac);
if (result && frac <= 1.0f)
{
if (frac < deepestFrac)
{
deepestFrac = frac;
groundPos = rayOrigin + frac * wheelDelta;
worldBody = body;
groundNormal = normal;
}
lastOnFloor = true;
}
}
if (!lastOnFloor)
{
return;
}
if (groundNormal.LengthSquared() > 0.0f)
{
groundNormal.Normalize();
}
// System.Diagnostics.Debug.WriteLine(groundPos.ToString());
displacement = rayLen * (1.0f - deepestFrac);
displacement = JMath.Clamp(displacement, 0.0f, WheelTravel);
float displacementForceMag = displacement * Spring;
// reduce force when suspension is par to ground
displacementForceMag *= Math.Abs(JVector.Dot(groundNormal, worldAxis));
// apply damping
float dampingForceMag = upSpeed * Damping;
float totalForceMag = displacementForceMag + dampingForceMag;
if (totalForceMag < 0.0f)
{
totalForceMag = 0.0f;
}
JVector extraForce = totalForceMag * worldAxis;
force += extraForce;
JVector groundUp = groundNormal;
JVector groundLeft = JVector.Cross(groundNormal, wheelFwd);
if (groundLeft.LengthSquared() > 0.0f)
{
groundLeft.Normalize();
}
JVector groundFwd = JVector.Cross(groundLeft, groundUp);
JVector wheelPointVel = car.LinearVelocity +
JVector.Cross(car.AngularVelocity, JVector.Transform(Position, car.Orientation));
// rimVel=(wxr)*v
JVector rimVel = angVel * JVector.Cross(wheelLeft, groundPos - worldPos);
wheelPointVel += rimVel;
JVector worldVel = worldBody.LinearVelocity +
JVector.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position);
wheelPointVel -= worldVel;
// sideways forces
float noslipVel = 0.1f;
float slipVel = 0.1f;
float slipFactor = 0.7f;
float smallVel = 3;
float friction = SideFriction;
float sideVel = JVector.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 = ForwardFriction;
float fwdVel = JVector.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;
// fwd force also spins the wheel
JVector wheelCentreVel = car.LinearVelocity +
JVector.Cross(car.AngularVelocity, JVector.Transform(Position, car.Orientation));
angVelForGrip = JVector.Dot(wheelCentreVel, groundFwd) / Radius;
torque += -fwdForce * Radius;
// add force to car
car.AddForce(force, groundPos + 0.5f * JVector.Up);
// add force to the world
if (!worldBody.IsStatic)
{
worldBody.AddForce(force * (-1) * 0.01f, groundPos);
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(float timestep)
{
if (body1.IsTrigger || body2.IsTrigger)
{
return;
}
float dvx, dvy;
// relative velocity at contact point
dvx = body2.linearVelocity.X + (-body2.angularVelocity * relativePos2.Y);
dvy = body2.linearVelocity.Y + (body2.angularVelocity * relativePos2.X);
dvx = dvx - body1.linearVelocity.X + (-body1.angularVelocity * relativePos1.Y);
dvy = dvy - body1.linearVelocity.Y + (body1.angularVelocity * relativePos1.X);
float kNormal = 0.0f;
float rantra = 0.0f;
// if body1 isn't static
if (!treatBody1AsStatic)
{
// add it's mass to the mass normal
kNormal += body1.inverseMass;
// if body1 isn't a mass point (particle)
if (!body1IsMassPoint)
{
//
rantra = relativePos1.X * normal.Y - relativePos1.Y * normal.X;
}
}
float rbntrb = 0.0f;
// if body2 isn't static
if (!treatBody2AsStatic)
{
// add it's mass to the mass normal
kNormal += body2.inverseMass;
// if body1 isn't a mass point (particle)
if (!body2IsMassPoint)
{
//
rbntrb = relativePos2.X * normal.Y - relativePos2.Y * normal.X;
}
}
// compute overall mass normal
if (!treatBody1AsStatic)
{
kNormal += body1.invInertia * (rantra * rantra);
}
if (!treatBody2AsStatic)
{
kNormal += body2.invInertia * (rbntrb * rbntrb);
}
massNormal = 1.0f / kNormal;
tangent.X = -normal.Y;
tangent.Y = normal.X;
float kTangent = 0.0f;
if (treatBody1AsStatic)
{
rantra = 0.0f;
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
//
rantra = relativePos1.X * tangent.Y - relativePos1.Y * tangent.X;
}
}
if (treatBody2AsStatic)
{
rbntrb = 0.0f;
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
rantra = relativePos2.X * tangent.Y - relativePos2.Y * tangent.X;
}
}
// compute overall mass tangent
if (!treatBody1AsStatic)
{
kTangent += body1.invInertia * (rantra * rantra);
}
if (!treatBody2AsStatic)
{
kTangent += body2.invInertia * (rbntrb * rbntrb);
}
massTangent = 1.0f / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = 0.0f;
float relNormalVel = JVector.Dot(normal, body2.linearVelocity + JVector.Cross(body2.angularVelocity, relativePos2) - body1.linearVelocity + JVector.Cross(body1.angularVelocity, relativePos1));
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (1.0f / timestep) * JMath.Max(0.0f, Penetration - settings.allowedPenetration);
restitutionBias = JMath.Clamp(restitutionBias, 0.0f, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
float timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
float relTangentVel = -(tangent.X * dvx + tangent.Y * dvy);
float tangentImpulse = massTangent * relTangentVel;
float maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
JVector impulse;
// Simultaneous solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -1.0f && newContact)
{
restitutionBias = Math.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = 0.0f;
}
else
{
lostSpeculativeBounce = 0.0f;
}
// warm start
impulse.X = normal.X * accumulatedNormalImpulse + tangent.X * accumulatedTangentImpulse;
impulse.Y = normal.Y * accumulatedNormalImpulse + tangent.Y * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.X -= (impulse.X * body1.inverseMass);
body1.linearVelocity.Y -= (impulse.Y * body1.inverseMass);
if (!body1IsMassPoint)
{
body1.angularVelocity -= body1.invInertia * (relativePos1.X * impulse.Y - relativePos1.Y * impulse.X);
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.X += (impulse.X * body2.inverseMass);
body2.linearVelocity.Y += (impulse.Y * body2.inverseMass);
if (!body2IsMassPoint)
{
body2.angularVelocity += body2.invInertia * (relativePos2.X * impulse.Y - relativePos2.Y * impulse.X);
}
}
lastTimeStep = timestep;
newContact = false;
}
public Vector3 GetPointVelocity(Vector3 point)
{
var velocity = Body.LinearVelocity + JVector.Cross(Body.AngularVelocity, point.ToJVector() - Body.Position);
return(velocity.ToVector3());
}
