/// <summary>
/// Checks if given point is within 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="point">The point to check.</param>
/// <returns>Returns true if the point is within the shape, otherwise false.</returns>
public static bool Pointcast(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector point)
{
JVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
JVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
JVector r; support.SupportCenter(out r);
JVector.Transform(ref r, ref orientation, out r);
JVector.Add(ref position, ref r, out r);
JVector.Subtract(ref point, ref r, out r);
JVector x = point;
JVector w, p;
float VdotR;
JVector v; JVector.Subtract(ref x, ref arbitraryPoint, out v);
float dist = v.LengthSquared();
float epsilon = 0.0001f;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > 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 * JMath.Epsilon))
{
simplexSolverPool.GiveBack(simplexSolver); return(false);
}
else
{
simplexSolver.Reset();
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
dist = v.LengthSquared();
}
else
{
dist = 0.0f;
}
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
this.Exit();
}
MouseState mouse = Mouse.GetState();
KeyboardState keys = Keyboard.GetState();
// mouse movement when left-button
if (mouse.LeftButton == ButtonState.Pressed)
{
float x = (lastMouse.X - mouse.X) * 0.2f;
float y = (lastMouse.Y - mouse.Y) * 0.2f;
paddle2.Position += new JVector(-x, y, 0f);
paddle1.Position += new JVector(-x, y, 0f);
}
// tilt controls for paddles
if (keys.IsKeyDown(Keys.Left))
{
JMatrix rotated =
Conversion.ToJitterMatrix(Matrix.CreateRotationY(MathHelper.ToRadians(1)));
paddle2.Orientation *= rotated;
paddle1.Orientation *= rotated;
}
if (keys.IsKeyDown(Keys.Right))
{
JMatrix rotated =
Conversion.ToJitterMatrix(Matrix.CreateRotationY(MathHelper.ToRadians(-1)));
paddle2.Orientation *= rotated;
paddle1.Orientation *= rotated;
}
if (keys.IsKeyDown(Keys.Up))
{
JMatrix rotated =
Conversion.ToJitterMatrix(Matrix.CreateRotationX(MathHelper.ToRadians(1)));
paddle2.Orientation *= rotated;
paddle1.Orientation *= rotated;
}
if (keys.IsKeyDown(Keys.Down))
{
JMatrix rotated =
Conversion.ToJitterMatrix(Matrix.CreateRotationX(MathHelper.ToRadians(-1)));
paddle2.Orientation *= rotated;
paddle1.Orientation *= rotated;
}
if (keys.IsKeyDown(Keys.Space))
{
ball.AddForce(JVector.Up * 100f);
}
// ball and paddle-orientation reset
if (keys.IsKeyDown(Keys.R))
{
ball.IsStatic = false;
ball.LinearVelocity = JVector.Zero;
ball.Position = new JVector(0f, 9f, 18f);
float x_variation = (float)(generator.Next(6000) - 3000);
ball.AddForce(new JVector(x_variation, 1000f, -10000f));
paddle1.Orientation = JMatrix.Identity;
paddle2.Orientation = JMatrix.Identity;
}
//player.HandleInput(gameTime);
world.Step(1.0f / 50.0f, true);
lastMouse = mouse;
base.Update(gameTime);
}
/// <summary>
/// Discrete Box vs Box test. Very fast. Generates contact info. NOTE: ensure UpdateAxes is called for each BoxShape prior.
/// </summary>
/// <param name="A">BoxShape A.</param>
/// <param name="PA">BoxShape A's position.</param>
/// <param name="OA">BoxShape A's orientation.</param>
/// <param name="B">BoxShape B.</param>
/// <param name="PB">BoxShape B's position.</param>
/// <param name="OB">BoxShape B's orientation.</param>
/// <param name="normal">Normal of collision.</param>
/// <param name="t">Amount of penetration.</param>
/// <param name="CA">Contacts found on BoxShape A.</param>
/// <param name="CB">Contacts found on BoxShape B.</param>
/// <param name="NumContacts">Number of contacts found.</param>
/// <returns>True if collision exists.</returns>
public static bool BoxBoxTestContact(ref BoxShape A, ref JVector PA, ref JMatrix OA,
ref BoxShape B, ref JVector PB, ref JMatrix OB,
out JVector normal, out float t, out JVector[] CA, out JVector[] CB, out int NumContacts)
{
float overlap = 0;
normal = A.xAxis;
t = 0.0f;
// TODO in future to save a few bytes of garbage make these
// SA1, SA2 and SB1, SB2 (no arrays)
JVector[] SA = new JVector[2], SB = new JVector[2];
int NumSA = 0, NumSB = 0;
CA = new JVector[2]; CB = new JVector[2];
NumContacts = 0;
JVector T = PB - PA;
// cache scaled local axes
JVector Ax = A.halfSize.X * A.xAxis;
JVector Ay = A.halfSize.Y * A.yAxis;
JVector Bx = B.halfSize.X * B.xAxis;
JVector By = B.halfSize.Y * B.yAxis;
// axis to test
JVector L = A.xAxis;
float TL = Math.Abs(T * L);
float a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
float b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
// just set first overlap
overlap = TL - (b + a);
// axis to test
L = A.yAxis;
TL = Math.Abs(T * L);
a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
var o = TL - (b + a);
if (o > overlap)
{
overlap = o;
normal = A.yAxis;
}
// axis to test
L = B.xAxis;
TL = Math.Abs(T * L);
a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
o = TL - (b + a);
if (o > overlap)
{
overlap = o;
normal = B.xAxis;
}
// axis to test
L = B.yAxis;
TL = Math.Abs(T * L);
a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
o = TL - (b + a);
if (o > overlap)
{
overlap = o;
normal = B.yAxis;
}
// make sure the polygons gets pushed away from each other.
if (normal * T < 0.0f)
{
normal = -normal;
}
t = overlap;
// now to find a contact point or edge!
var mn = -normal;
NumSA = A.FindSupportPoints(ref mn, ref PA, ref OA, out SA);
NumSB = B.FindSupportPoints(ref normal, ref PB, ref OB, out SB);
// now refine contact points from support points
// edge/vertex case
if (NumSA == 2 && NumSB == 1)
{
ProjectPointOnLine(ref SB[0], ref SA[0], ref SA[1], out CA[NumContacts]);
CB[NumContacts] = SB[0];
NumContacts++;
return(true);
}
// vertex/edge case
if (NumSA == 1 && NumSB == 2)
{
ProjectPointOnLine(ref SA[0], ref SB[0], ref SB[1], out CB[NumContacts]);
CA[NumContacts] = SA[0];
NumContacts++;
return(true);
}
// edge/edge case
if (NumSA == 2 && NumSB == 2)
{
// clip contacts
JVector perp = new JVector(-normal.Y, normal.X);
// project first 2 contacts to axis perpendicular to normal
float min0 = SA[0] * perp;
float max0 = min0;
float min1 = SB[0] * perp;
float max1 = min1;
// project next point from A
max0 = SA[1] * perp;
if (max0 < min0)
{
JMath.Swapf(ref min0, ref max0);
JVector.Swap(ref SA[0], ref SA[1]);
}
max1 = SB[1] * perp;
if (max1 < min1)
{
JMath.Swapf(ref min1, ref max1);
JVector.Swap(ref SB[0], ref SB[1]);
}
if (min0 > min1)
{
JVector Pseg;
ProjectPointOnLine(ref SA[0], ref SB[0], ref SB[1], out Pseg);
CA[NumContacts] = SA[0];
CB[NumContacts] = Pseg;
NumContacts++;
}
else
{
JVector Pseg;
ProjectPointOnLine(ref SB[0], ref SA[0], ref SA[1], out Pseg);
CA[NumContacts] = Pseg;
CB[NumContacts] = SB[0];
NumContacts++;
}
if (max0 < max1)
{
JVector Pseg;
ProjectPointOnLine(ref SA[1], ref SB[0], ref SB[1], out Pseg);
CA[NumContacts] = SA[1];
CB[NumContacts] = Pseg;
NumContacts++;
}
else
{
JVector Pseg;
ProjectPointOnLine(ref SB[1], ref SA[0], ref SA[1], out Pseg);
CA[NumContacts] = Pseg;
CB[NumContacts] = SB[1];
NumContacts++;
}
return(true);
}
// if all axes overlap collision exists
return(true);
}
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
//ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB,
//out JVector p1, out JVector p2, out JVector normal)
// {
// VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
// simplexSolver.Reset();
// float lambda = 0.0f;
// p1 = p2 = JVector.Zero;
// JVector x1 = position1;
// JVector x2 = position2;
// JVector r = sweptA - sweptB;
// JVector w, v;
// JVector supVertexA;
// JVector rn = JVector.Negate(r);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA);
// JVector supVertexB;
// SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB);
// v = supVertexA - supVertexB;
// bool hasResult = false;
// normal = JVector.Zero;
// float lastLambda = lambda;
// int maxIter = MaxIterations;
// float distSq = v.LengthSquared();
// float epsilon = 0.00001f;
// float VdotR;
// while ((distSq > epsilon) && (maxIter-- != 0))
// {
// JVector vn = JVector.Negate(v);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA);
// SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB);
// w = supVertexA - supVertexB;
// 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;
// x1 = position1 + lambda * sweptA;
// x2 = position2 + lambda * sweptB;
// w = supVertexA - supVertexB;
// normal = v;
// hasResult = true;
// }
// }
// if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
// if (simplexSolver.Closest(out v))
// {
// distSq = v.LengthSquared();
// normal = v;
// hasResult = true;
// }
// else distSq = 0.0f;
// }
// simplexSolver.ComputePoints(out p1, out p2);
// if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
// normal.Normalize();
// p1 = p1 - lambda * sweptA;
// p2 = p2 - lambda * sweptB;
// simplexSolverPool.GiveBack(simplexSolver);
// return true;
// }
#endregion
// 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>
/// Constraints two bodies to always have the same relative
/// orientation to each other. Combine the AngleConstraint with a PointOnLine
/// Constraint to get a prismatic joint.
/// </summary>
public FixedAngle(RigidBody body1)
: base(body1, null)
{
orientation = body1.orientation;
}
/// <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;
}
}
}
}
}
//int sampleCount = 50;
//float distance = 4.0f;
public void UpdateAmbientOcclusion(Model model, Group occluders, string mapName)
{
#if false // \todo fix
GeometryMesh mesh = model.Batch.MeshSource as GeometryMesh;
if (mesh == null)
{
return;
}
Geometry geometry = mesh.Geometry;
model.UpdateOctree();
var shape = new TriangleMeshShape(model.Octree);
shape.SphericalExpansion = 0.0f;
#if VISUALIZE_RAYS
sceneManager.DebugLineRenderer.Begin();
#endif
var pointLocations = geometry.PointAttributes.Find <Vector3>("point_locations");
var polygonCentroids = geometry.PolygonAttributes.Find <Vector3>("polygon_centroids");
var pointNormals = geometry.PointAttributes.Find <Vector3>(mapName);
var cornerNormals = geometry.CornerAttributes.Contains <Vector3>("corner_normals") ? geometry.CornerAttributes.Find <Vector3>("corner_normals") : null;
var cornerColors = geometry.CornerAttributes.FindOrCreate <Vector4>("corner_colors");
cornerColors.Clear();
int badCount = 0;
foreach (Polygon polygon in geometry.Polygons)
{
foreach (Corner corner in polygon.Corners)
{
Point point = corner.Point;
// Start from corner position slightly moved towards polygon center
Vector3 initialEye = Vector3.Mix(pointLocations[point], polygonCentroids[polygon], 0.001f);
Vector3 normal;
if (
(cornerNormals != null) &&
(cornerNormals.ContainsKey(corner) == true)
)
{
normal = cornerNormals[corner];
}
else
{
normal = pointNormals[point];
}
JMatrix orientation = JMatrix.Identity;
JMatrix invOrientation = JMatrix.Identity;
Vector3 modelPosition = Vector3.Zero;
// Slightly up from surface
initialEye += normal * 0.01f;
int visibility = sampleCount;
JVector position = Conversions.JVector(modelPosition);
JVector direction;
JVector jnormal;
Vector3 eye = initialEye;
JVector origin = Conversions.JVector(eye);
// Sample a few directions
for (int c = 0; c < sampleCount; ++c)
{
// Get a random direction until it points up from the surface
Vector3 sampleDirection;
float dot;
do
{
sampleDirection = Random();
dot = Vector3.Dot(normal, sampleDirection);
}while(dot < 0.25f);
// Scale
direction = Conversions.JVector(sampleDirection * distance);
bool hit = false;
{
float tempFraction;
if (shape is Multishape)
{
Multishape ms = (shape as Multishape).RequestWorkingClone();
JVector tempNormal;
int msLength = ms.Prepare(ref origin, ref direction);
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (
GJKCollide.Raycast(
ms, ref orientation, ref invOrientation, ref position,
ref origin, ref direction, out tempFraction, out tempNormal
)
)
{
hit = true;
break;
}
}
ms.ReturnWorkingClone();
}
else
{
hit = GJKCollide.Raycast(
shape, ref orientation, ref invOrientation, ref position,
ref origin, ref direction, out tempFraction, out jnormal
);
}
}
if (hit)
{
visibility -= 1;
}
Vector3 root = new Vector3(origin.X, origin.Y, origin.Z);
Vector3 tip = root + 0.02f * new Vector3(direction.X, direction.Y, direction.Z);
#if VISUALIZE_RAYS
sceneManager.DebugLineRenderer.Line(
root,
Vector4.Zero,
tip,
(hit ? Vector4.UnitX : Vector4.UnitY)
);
#endif
}
float visibilityFloat = (float)(visibility) / (float)(sampleCount);
cornerColors[corner] = new Vector4(visibilityFloat, visibilityFloat, visibilityFloat, 1.0f);
}
}
mesh.Geometry.ComputePolygonCentroids();
mesh.Geometry.ComputePolygonNormals();
mesh.Geometry.SmoothNormalize("corner_normals", "polygon_normals", (2.0f * (float)Math.PI));
mesh.Geometry.SmoothAverage("corner_colors", mapName);
mesh.BuildMeshFromGeometry(BufferUsageHint.StaticDraw, NormalStyle.CornerNormals);
//UpdateMeshCornerColors(mesh);
Debug.WriteLine("bad count = " + badCount.ToString());
#if VISUALIZE_RAYS
sceneManager.DebugLineRenderer.End();
sceneManager.DebugFrame = model.Frame;
#endif
//model.Name = "AmbientOcclusion(" + model.Name + ")";
#endif
}
/// <summary>
/// Adds a body to the fluid. Only bodies which where added
/// to the fluidvolume gets affected by buoyancy forces.
/// </summary>
/// <param name="body">The body which should be added.</param>
/// <param name="subdivisions">The object is subdivided in smaller objects
/// for which buoyancy force is calculated. The more subdivisons the better
/// the results. Note that the total number of subdivisions is subdivisions³.</param>
public void Add(RigidBody body, int subdivisions)
{
List <JVector> massPoints = new List <JVector>();
JVector testVector;
JVector diff = body.Shape.BoundingBox.Max - body.Shape.BoundingBox.Min;
if (diff.IsNearlyZero())
{
throw new InvalidOperationException("BoundingBox volume of the shape is zero.");
}
Multishape ms = body.Shape as Multishape;
int values = 0;
if (ms != null)
{
JBBox largeBox = JBBox.LargeBox;
values = ms.Prepare(ref largeBox);
}
for (int i = 0; i < subdivisions; i++)
{
for (int e = 0; e < subdivisions; e++)
{
for (int k = 0; k < subdivisions; k++)
{
testVector.X = body.Shape.BoundingBox.Min.X + (diff.X / (float)(subdivisions - 1)) * ((float)i);
testVector.Y = body.Shape.BoundingBox.Min.Y + (diff.Y / (float)(subdivisions - 1)) * ((float)e);
testVector.Z = body.Shape.BoundingBox.Min.Z + (diff.Z / (float)(subdivisions - 1)) * ((float)k);
JMatrix ident = JMatrix.Identity;
JVector zero = JVector.Zero;
if (ms != null)
{
for (int j = 0; j < values; j++)
{
ms.SetCurrentShape(j);
if (GJKCollide.Pointcast(body.Shape, ref ident,
ref zero, ref testVector))
{
massPoints.Add(testVector);
}
}
}
else
{
if (GJKCollide.Pointcast(body.Shape, ref ident,
ref zero, ref testVector))
{
massPoints.Add(testVector);
}
}
}
}
}
samples.Add(body.Shape, massPoints.ToArray());
bodies.Add(body);
}
protected override void Update(GameTime gameTime)
{
padState = GamePad.GetState(PlayerIndex.One);
keyState = Keyboard.GetState();
mouseState = Mouse.GetState();
// let the user escape the demo
if (PressedOnce(Keys.Escape, Buttons.Back))
{
this.Exit();
}
// change threading mode
if (PressedOnce(Keys.M, Buttons.A))
{
multithread = !multithread;
}
if (PressedOnce(Keys.P, Buttons.A))
{
var e = World.RigidBodies.GetEnumerator();
e.MoveNext(); e.MoveNext();
World.RemoveBody(e.Current as RigidBody);
}
#region drag and drop physical objects with the mouse
if (mouseState.LeftButton == ButtonState.Pressed &&
mousePreviousState.LeftButton == ButtonState.Released ||
padState.IsButtonDown(Buttons.RightThumbstickDown) &&
gamePadPreviousState.IsButtonUp(Buttons.RightThumbstickUp))
{
JVector ray = Conversion.ToJitterVector(RayTo(mouseState.X, mouseState.Y));
JVector camp = Conversion.ToJitterVector(Camera.Position);
ray = JVector.Normalize(ray) * 100;
float fraction;
bool result = World.CollisionSystem.Raycast(camp, ray, RaycastCallback, out grabBody, out hitNormal, out fraction);
if (result)
{
hitPoint = camp + fraction * ray;
if (grabConstraint != null)
{
World.RemoveConstraint(grabConstraint);
}
JVector lanchor = hitPoint - grabBody.Position;
lanchor = JVector.Transform(lanchor, JMatrix.Transpose(grabBody.Orientation));
grabConstraint = new SingleBodyConstraints.PointOnPoint(grabBody, lanchor);
grabConstraint.Softness = 0.01f;
grabConstraint.BiasFactor = 0.1f;
World.AddConstraint(grabConstraint);
hitDistance = (Conversion.ToXNAVector(hitPoint) - Camera.Position).Length();
scrollWheel = mouseState.ScrollWheelValue;
grabConstraint.Anchor = hitPoint;
}
}
if (mouseState.LeftButton == ButtonState.Pressed || padState.IsButtonDown(Buttons.RightThumbstickDown))
{
hitDistance += (mouseState.ScrollWheelValue - scrollWheel) * 0.01f;
scrollWheel = mouseState.ScrollWheelValue;
if (grabBody != null)
{
Vector3 ray = RayTo(mouseState.X, mouseState.Y); ray.Normalize();
grabConstraint.Anchor = Conversion.ToJitterVector(Camera.Position + ray * hitDistance);
grabBody.IsActive = true;
if (!grabBody.IsStatic)
{
grabBody.LinearVelocity *= 0.98f;
grabBody.AngularVelocity *= 0.98f;
}
}
}
else
{
if (grabConstraint != null)
{
World.RemoveConstraint(grabConstraint);
}
grabBody = null;
grabConstraint = null;
}
#endregion
#region create random primitives
if (PressedOnce(Keys.Space, Buttons.B))
{
SpawnRandomPrimitive(Conversion.ToJitterVector(Camera.Position),
Conversion.ToJitterVector((Camera.Target - Camera.Position) * 40.0f));
}
#endregion
#region switch through physic scenes
if (PressedOnce(Keys.Add, Buttons.X))
{
DestroyCurrentScene();
currentScene++;
currentScene = currentScene % PhysicScenes.Count;
PhysicScenes[currentScene].Build();
}
if (PressedOnce(Keys.Subtract, Buttons.Y))
{
DestroyCurrentScene();
currentScene += PhysicScenes.Count - 1;
currentScene = currentScene % PhysicScenes.Count;
PhysicScenes[currentScene].Build();
}
#endregion
UpdateDisplayText(gameTime);
float step = (float)gameTime.ElapsedGameTime.TotalSeconds;
if (step > 1.0f / 100.0f)
{
step = 1.0f / 100.0f;
}
World.Step(step, multithread);
gamePadPreviousState = padState;
keyboardPreviousState = keyState;
mousePreviousState = mouseState;
base.Update(gameTime);
}
public static quat ToQuat(this JMatrix m)
{
var q = JQuaternion.CreateFromMatrix(m);
return(new quat(q.X, q.Y, q.Z, q.W));
}
public override void PrepareForIteration(float timestep)
{
// send a ray from our feet position down.
// if we collide with something which is 0.05f units below our feets remember this!
RigidBody resultingBody = null;
JVector normal;
float frac;
var rayOrigin = Body1.Position + JVector.Down * (feetPosition - 0.1f);
bool result = JPhysics.World.CollisionSystem.Raycast(
rayOrigin,
JVector.Down,
(body, hitNormal, fraction) => body != Body1,
out resultingBody,
out normal,
out frac);
if (BodyWalkingOn != null)
{
contactPoint = rayOrigin + JVector.Down * frac;
localContactPoint = JVector.Transform(contactPoint - BodyWalkingOn.Position, JMatrix.Inverse(BodyWalkingOn.Orientation));
}
BodyWalkingOn = (result && frac <= 0.2f) ? resultingBody : null;
shouldIJump = TryJump && result && (frac <= 0.2f) && (Body1.LinearVelocity.Y < JumpVelocity);
}
/// <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(World 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 = 100.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;
}
internal static bool CircleBoxTest(ref CircleShape A, ref JVector PA, ref BoxShape B, ref JVector PB, ref JMatrix OB)
{
// find vertex closest to circles center
// move circle into boxes space
var pa = JVector.TransposedTransform(PA - PB, OB);
// find normal from box to circles center
var axis = pa;
JVector closestVertex;
// find closest vertex
B.SupportMapping(ref axis, out closestVertex);
// do a SAT test on that axis
// axis to test
JVector T = pa - closestVertex;
float TL = Math.Abs(T * axis);
float a = Math.Abs(pa * axis);
float b = Math.Abs(closestVertex * axis);
if (TL > (a + b + A.Radius))
{
return(false);
}
return(true);
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
this.Exit();
}
KeyboardState keys = Keyboard.GetState();
JVector moveVector = JVector.Zero;
float amountOfMovement = 0.05f;
if (keys.IsKeyDown(Keys.Right))
{
moveVector.X += amountOfMovement;
}
if (keys.IsKeyDown(Keys.Left))
{
moveVector.X -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Down))
{
moveVector.Y -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Up))
{
moveVector.Y += amountOfMovement;
}
body1.Position += moveVector;
if (keys.IsKeyDown(Keys.OemPlus) && oldState.IsKeyUp(Keys.OemPlus))
{
GJKCollide.MaxIterations++;
}
if (keys.IsKeyDown(Keys.OemMinus) && oldState.IsKeyUp(Keys.OemMinus))
{
GJKCollide.MaxIterations--;
}
GJKCollide.MaxIterations = (int)JMath.Clamp(GJKCollide.MaxIterations, 0, 25);
bool changeShape = false;
if (keys.IsKeyDown(Keys.D1) && oldState.IsKeyUp(Keys.D1))
{
shapeType++;
changeShape = true;
}
if (keys.IsKeyDown(Keys.D2) && oldState.IsKeyUp(Keys.D2))
{
shapeType--;
changeShape = true;
}
shapeType = (int)JMath.Clamp(shapeType, 0, 2);
if (changeShape)
{
Random r = new Random();
switch (shapeType)
{
case 0: // circle
body1 = new RigidBody(new CircleShape((float)r.NextDouble() * 3f));
break;
case 1: // capsule
body1 = new RigidBody(new CapsuleShape((float)r.NextDouble() * 3f, (float)r.NextDouble() * 1f));
break;
case 2: // box
body1 = new RigidBody(new BoxShape((float)r.NextDouble() * 3f, (float)r.NextDouble() * 3f));
break;
}
}
body1.Orientation += 0.005f;
body2.Orientation -= 0.005f;
JMatrix o1 = JMatrix.CreateRotationZ(body1.Orientation);
JMatrix o2 = JMatrix.CreateRotationZ(body2.Orientation);
JVector pos1 = body1.Position;
JVector pos2 = body2.Position;
JVector point2;
sw.Start();
hit = GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref o1, ref o2, ref pos1, ref pos2, out point, out point2, out normal);
sw.Stop();
penetration = JVector.Distance(point, point2);
ticks = sw.ElapsedTicks;
sw.Reset();
DebugDrawer.DrawPoint(point2);
DebugDrawer.DrawPoint(point);
DebugDrawer.Color = Color.Red;
DebugDrawer.DrawLine(JVector.Zero, normal);
DebugDrawer.Color = Color.Black;
DebugDrawer.DrawLine(JVector.Up, JVector.Down);
DebugDrawer.DrawLine(JVector.Left, JVector.Right);
body1.DebugDraw(DebugDrawer);
body2.DebugDraw(DebugDrawer);
oldState = keys;
base.Update(gameTime);
}
private static JMatrix to_j(Quaternion r)
{
var q = new JQuaternion(r.X, r.Y, r.Z, r.W);
return(JMatrix.CreateFromQuaternion(q));
}
/// <summary>
/// Calculates the inertia of the shape relative to the center of mass.
/// </summary>
/// <param name="shape"></param>
/// <param name="centerOfMass"></param>
/// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param>
/// <returns></returns>
#region public static float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia)
public static float CalculateMassInertia(Shape shape, out JVector centerOfMass,
out JMatrix inertia)
{
float mass = 0.0f;
centerOfMass = JVector.Zero; inertia = JMatrix.Zero;
if (shape is Multishape)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
// build a triangle hull around the shape
List <JVector> hullTriangles = new List <JVector>();
shape.MakeHull(ref hullTriangles, 3);
// create inertia of tetrahedron with vertices at
// (0,0,0) (1,0,0) (0,1,0) (0,0,1)
float a = 1.0f / 60.0f, b = 1.0f / 120.0f;
JMatrix C = new JMatrix(a, b, b, b, a, b, b, b, a);
for (int i = 0; i < hullTriangles.Count; i += 3)
{
JVector column0 = hullTriangles[i + 0];
JVector column1 = hullTriangles[i + 1];
JVector column2 = hullTriangles[i + 2];
JMatrix A = new JMatrix(column0.X, column1.X, column2.X,
column0.Y, column1.Y, column2.Y,
column0.Z, column1.Z, column2.Z);
float detA = A.Determinant();
// now transform this canonical tetrahedron to the target tetrahedron
// inertia by a linear transformation A
JMatrix tetrahedronInertia = JMatrix.Multiply(A * C * JMatrix.Transpose(A), detA);
JVector tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]);
float tetrahedronMass = (1.0f / 6.0f) * detA;
inertia += tetrahedronInertia;
centerOfMass += tetrahedronMass * tetrahedronCOM;
mass += tetrahedronMass;
}
inertia = JMatrix.Multiply(JMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass = centerOfMass * (1.0f / mass);
float x = centerOfMass.X;
float y = centerOfMass.Y;
float z = centerOfMass.Z;
// now translate the inertia by the center of mass
JMatrix t = new JMatrix(
-mass * (y * y + z * z), mass * x * y, mass * x * z,
mass * y * x, -mass * (z * z + x * x), mass * y * z,
mass * z * x, mass * z * y, -mass * (x * x + y * y));
JMatrix.Add(ref inertia, ref t, out inertia);
return(mass);
}
internal static Quaternion ToArenaData(JMatrix v)
{
var q = JQuaternion.CreateFromMatrix(v);
return(ToArenaData(q));
}
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);
var force = JVector.Zero;
var worldAxis = JVector.Transform(JVector.Up, car.Orientation);
var worldPos = car.Position + JVector.Transform(Position, car.Orientation);
var forward = new JVector(-car.Orientation.M31, -car.Orientation.M32, -car.Orientation.M33);
var wheelFwd = JVector.Transform(forward, JMatrix.CreateFromAxisAngle(JVector.Up, SteerAngle / 360 * 2 * JMath.Pi));
var wheelLeft = JVector.Cross(JVector.Up, wheelFwd); wheelLeft.Normalize();
var wheelUp = JVector.Cross(wheelFwd, wheelLeft);
float rayLen = (2.0f * Radius) + WheelTravel;
var wheelRayStart = worldPos;
var wheelDelta = -Radius * worldAxis;
var wheelRayEnd = worldPos + wheelDelta;
float deltaFwd = 2.0f * Radius / (NumberOfRays + 1);
float deltaFwdStart = deltaFwd;
lastDisplacement = displacement;
displacement = 0.0f;
lastOnFloor = false;
var rayOrigin = car.Position + JVector.Transform(Position, car.Orientation);
var groundNormal = JVector.Zero;
var 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)));
var newOrigin = wheelRayStart + (distFwd * wheelFwd) + (zOffset * wheelUp);
bool result = world.CollisionSystem.Raycast(newOrigin, wheelDelta,
raycast, out var body, out var normal, out float 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;
}
var extraForce = totalForceMag * worldAxis;
force += extraForce;
var groundUp = groundNormal;
var groundLeft = JVector.Cross(groundNormal, wheelFwd);
if (groundLeft.LengthSquared() > 0.0f)
{
groundLeft.Normalize();
}
var groundFwd = JVector.Cross(groundLeft, groundUp);
var wheelPointVel = car.LinearVelocity
+ JVector.Cross(car.AngularVelocity, JVector.Transform(Position, car.Orientation));
// rimVel=(wxr)*v
var rimVel = angVel * JVector.Cross(wheelLeft, groundPos - worldPos);
wheelPointVel += rimVel;
var 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
var 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>
/// Log a matrix from the jitter-physics
/// (Andy: Please do not remove this, also when unusused since it might come handy to quickly debug something, thanks)
/// </summary>
/// <param name="m">Matrix to log on the console</param>
/// <param name="info">Additional info to print in front</param>
public static void Log(JMatrix m, string info = "")
{
string s = $"{info}:\n{m.M11} {m.M12} {m.M13}\n{m.M21} {m.M22} {m.M23}\n{m.M31} {m.M32} {m.M33}";
Log(s);
}
public static void ExtractData(List <JVector> vertices, List <TriangleVertexIndices> indices, Model model)
{
Matrix[] bones_ = new Matrix[model.Bones.Count];
model.CopyAbsoluteBoneTransformsTo(bones_);
foreach (ModelMesh modelmesh in model.Meshes)
{
JMatrix xform = Conversion.ToJitterMatrix(bones_[modelmesh.ParentBone.Index]);
foreach (ModelMeshPart meshPart in modelmesh.MeshParts)
{
// Before we add any more where are we starting from
int offset = vertices.Count;
// Read the format of the vertex buffer
VertexDeclaration declaration = meshPart.VertexBuffer.VertexDeclaration;
VertexElement[] vertexElements = declaration.GetVertexElements();
// Find the element that holds the position
VertexElement vertexPosition = new VertexElement();
foreach (VertexElement vert in vertexElements)
{
if (vert.VertexElementUsage == VertexElementUsage.Position &&
vert.VertexElementFormat == VertexElementFormat.Vector3)
{
vertexPosition = vert;
// There should only be one
break;
}
}
// Check the position element found is valid
if (vertexPosition == null ||
vertexPosition.VertexElementUsage != VertexElementUsage.Position ||
vertexPosition.VertexElementFormat != VertexElementFormat.Vector3)
{
throw new Exception("Model uses unsupported vertex format!");
}
// This where we store the vertices until transformed
JVector[] allVertex = new JVector[meshPart.NumVertices];
// Read the vertices from the buffer in to the array
meshPart.VertexBuffer.GetData <JVector>(
meshPart.VertexOffset * declaration.VertexStride + vertexPosition.Offset,
allVertex,
0,
meshPart.NumVertices,
declaration.VertexStride);
// Transform them based on the relative bone location and the world if provided
for (int i = 0; i != allVertex.Length; ++i)
{
JVector.Transform(ref allVertex[i], ref xform, out allVertex[i]);
}
// Store the transformed vertices with those from all the other meshes in this model
vertices.AddRange(allVertex);
// Find out which vertices make up which triangles
if (meshPart.IndexBuffer.IndexElementSize != IndexElementSize.SixteenBits)
{
// This could probably be handled by using int in place of short but is unnecessary
throw new Exception("Model uses 32-bit indices, which are not supported.");
}
// Each primitive is a triangle
short[] indexElements = new short[meshPart.PrimitiveCount * 3];
meshPart.IndexBuffer.GetData <short>(
meshPart.StartIndex * 2,
indexElements,
0,
meshPart.PrimitiveCount * 3);
// Each TriangleVertexIndices holds the three indexes to each vertex that makes up a triangle
TriangleVertexIndices[] tvi = new TriangleVertexIndices[meshPart.PrimitiveCount];
for (int i = 0; i != tvi.Length; ++i)
{
// The offset is because we are storing them all in the one array and the
// vertices were added to the end of the array.
tvi[i].I0 = indexElements[i * 3 + 0] + offset;
tvi[i].I1 = indexElements[i * 3 + 1] + offset;
tvi[i].I2 = indexElements[i * 3 + 2] + offset;
}
// Store our triangles
indices.AddRange(tvi);
}
}
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector p1, out JVector p2, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = JVector.Zero;
JVector r = position1 - position2;
JVector w, v;
JVector supVertexA;
JVector rn, vn;
rn = JVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
JVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = JVector.Zero;
int maxIter = 15;
float distSq = v.LengthSquared();
float epsilon = 0.00001f;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = JVector.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
normal = v;
}
else
{
distSq = 0.0f;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
protected virtual void Update()
{
position = cacheTransform.position.ConvertToJVector();
orientation = cacheTransform.rotation.ConvertToJMatrix();
}
/// <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 v01, v02, v0;
JVector v11, v12, v1;
JVector v21, v22, v2;
JVector v31, v32, v3;
JVector mn;
// Initialization of the output
point = normal = JVector.Zero;
penetration = 0.0f;
// 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);
}
// 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
normal = OutsidePortal(v1, v0);
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);
//LD.Draw(Conversion.ToXNAVector2(v1), Conversion.ToXNAVector2(v0), Color.Blue);
//LD.Draw(Conversion.ToXNAVector2(v2), Conversion.ToXNAVector2(v0), Color.Blue);
if (JVector.Dot(ref v2, ref normal) <= 0.0f)
{
return(false);
}
// phase two: portal refinement
int maxIterations = 0;
while (true)
{
// find normal direction
if (!IntersectPortal(v0, v2, v1))
{
normal = InsidePortal(v2, v1);
}
else
{
// origin ray crosses the portal
normal = OutsidePortal(v2, v1);
}
// obtain the next support point
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);
//LD.Draw(Conversion.ToXNAVector2(v3), Conversion.ToXNAVector2(v0), Color.Green);
if (JVector.Dot(v3, normal) <= 0)
{
JVector ab = v3 - v2;
float t = -(JVector.Dot(v2, ab)) / (JVector.Dot(ab, ab));
normal = (v2 + (t * ab));
return(false);
}
// Portal lies on the outside edge of the Minkowski Hull.
// Return contact information
if (JVector.Dot((v3 - v2), normal) <= CollideEpsilon || ++maxIterations > MaximumIterations)
{
JVector ab = v2 - v1;
float t = JVector.Dot(JVector.Negate(v1), ab);
if (t <= 0.0f)
{
t = 0.0f;
normal = v1;
}
else
{
float denom = JVector.Dot(ab, ab);
if (t >= denom)
{
normal = v2;
t = 1.0f;
}
else
{
t /= denom;
normal = v1 + t * ab;
}
}
float s = 1 - t;
point = s * v11 + t * v21;
var point2 = s * v12 + t * v22;
// this causes a sq root = bad!
penetration = normal.Length();
normal.Normalize();
return(true);
}
// if origin is inside (v1, v0, v3), refine portal
if (OriginInTriangle(v0, v1, v3))
{
v2 = v3;
v21 = v31;
v22 = v32;
continue;
}
// if origin is inside (v3, v0, v2), refine portal
else if (OriginInTriangle(v0, v2, v3))
{
v1 = v3;
v11 = v31;
v12 = v32;
continue;
}
return(false);
}
}
