/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
FPMatrix abs; FPMath.Absolute(ref orientation, out abs);
FPVector temp;
FPVector.Transform(ref halfSize, ref abs, out temp);
box.max = temp;
FPVector.Negate(ref temp, out box.min);
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref FPVector point, ref FPVector normal,
out FPVector point1, out FPVector point2)
{
FPVector mn; FPVector.Negate(ref normal, out mn);
FPVector sA; SupportMapping(body1, shape1, ref mn, out sA);
FPVector sB; SupportMapping(body2, shape2, ref normal, out sB);
FPVector.Subtract(ref sA, ref point, out sA);
FPVector.Subtract(ref sB, ref point, out sB);
FP dot1 = FPVector.Dot(ref sA, ref normal);
FP dot2 = FPVector.Dot(ref sB, ref normal);
FPVector.Multiply(ref normal, dot1, out sA);
FPVector.Multiply(ref normal, dot2, out sB);
FPVector.Add(ref point, ref sA, out point1);
FPVector.Add(ref point, ref sB, out point2);
}
private void CollisionDetected(RigidBody body1, RigidBody body2, FPVector point1, FPVector point2, FPVector normal, FP penetration)
{
bool anyBodyColliderOnly = body1.IsColliderOnly || body2.IsColliderOnly;
Arbiter arbiter = null;
ArbiterMap selectedArbiterMap = null;
if (anyBodyColliderOnly)
{
selectedArbiterMap = arbiterTriggerMap;
}
else
{
selectedArbiterMap = arbiterMap;
}
bool arbiterCreated = false;
lock (selectedArbiterMap) {
selectedArbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1; arbiter.body2 = body2;
selectedArbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
arbiterCreated = true;
}
}
Contact contact = null;
if (arbiter.body1 == body1)
{
FPVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (arbiterCreated)
{
if (anyBodyColliderOnly)
{
/*if (body1.isColliderOnly) {
* events.RaiseTriggerBeginCollide(body1, body2);
* } else {
* events.RaiseTriggerBeginCollide(body2, body1);
* }*/
events.RaiseTriggerBeginCollide(contact);
body1.arbitersTrigger.Add(arbiter);
body2.arbitersTrigger.Add(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialTriggers.Add(overlapContact);
}
else
{
events.RaiseBodiesBeginCollide(contact);
addedArbiterQueue.Enqueue(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialCollisions.Add(overlapContact);
}
}
if (!anyBodyColliderOnly && contact != null)
{
events.RaiseContactCreated(contact);
}
}
/// <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 FPMatrix orientation1,
ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2,
out FPVector point, out FPVector normal, out FP penetration)
{
// Used variables
FPVector temp1, temp2;
FPVector v01, v02, v0;
FPVector v11, v12, v1;
FPVector v21, v22, v2;
FPVector v31, v32, v3;
FPVector v41 = FPVector.zero, v42 = FPVector.zero, v4 = FPVector.zero;
FPVector mn;
// Initialization of the output
point = normal = FPVector.zero;
penetration = FP.Zero;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
FPVector.Transform(ref v01, ref orientation1, out v01);
FPVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
FPVector.Transform(ref v02, ref orientation2, out v02);
FPVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
FPVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new FPVector(FP.EN4, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
FPVector.Negate(ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
FPVector.Subtract(ref v12, ref v11, out v1);
if (FPVector.Dot(ref v1, ref normal) <= FP.Zero)
{
return(false);
}
// v2 = support perpendicular to v1,v0
FPVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
FPVector.Subtract(ref v1, ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
normal.Normalize();
point = v11;
FPVector.Add(ref point, ref v12, out point);
FPVector.Multiply(ref point, FP.Half, out point);
FPVector.Subtract(ref v12, ref v11, out temp1);
penetration = FPVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
FPVector.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);
FPVector.Subtract(ref v22, ref v21, out v2);
if (FPVector.Dot(ref v2, ref normal) <= FP.Zero)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
FP dist = FPVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > FP.Zero)
{
FPVector.Swap(ref v1, ref v2);
FPVector.Swap(ref v11, ref v21);
FPVector.Swap(ref v12, ref v22);
FPVector.Negate(ref normal, out normal);
UnityEngine.Debug.Log("normal: " + 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
FPVector.Negate(ref normal, out mn);
//UnityEngine.Debug.Log("mn: " + mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
FPVector.Subtract(ref v32, ref v31, out v3);
if (FPVector.Dot(ref v3, ref normal) <= FP.Zero)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
FPVector.Cross(ref v1, ref v3, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v2 = v3;
v21 = v31;
v22 = v32;
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v3, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
FPVector.Cross(ref v3, ref v2, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v1 = v3;
v11 = v31;
v12 = v32;
FPVector.Subtract(ref v3, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
/*
* UnityEngine.Debug.LogError(" ::Start STATE");
* UnityEngine.Debug.Log(temp1 + " " + temp2);
* UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0);
* UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1);
* UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2);
* UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3);
* UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4);
* UnityEngine.Debug.Log( mn);
*
* UnityEngine.Debug.LogError(" ::END STATE");
*/
// Compute normal of the wedge face
FPVector.Subtract(ref v2, ref v1, out temp1);
FPVector.Subtract(ref v3, ref v1, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// Beginer
// UnityEngine.Debug.Log("normal: " + normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
//UnityEngine.Debug.Log("normal: " + normal);
// Compute distance from origin to wedge face
FP d = FPVector.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
FPVector.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);
FPVector.Subtract(ref v42, ref v41, out v4);
FPVector.Subtract(ref v4, ref v3, out temp1);
FP delta = FPVector.Dot(ref temp1, ref normal);
penetration = FPVector.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 <= FP.Zero || phase2 > MaximumIterations)
{
if (hit)
{
FPVector.Cross(ref v1, ref v2, out temp1);
FP b0 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v3, ref v2, out temp1);
FP b1 = FPVector.Dot(ref temp1, ref v0);
FPVector.Cross(ref v0, ref v1, out temp1);
FP b2 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v2, ref v1, out temp1);
FP b3 = FPVector.Dot(ref temp1, ref v0);
FP sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
FPVector.Cross(ref v2, ref v3, out temp1);
b1 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v3, ref v1, out temp1);
b2 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v1, ref v2, out temp1);
b3 = FPVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
FP inv = FP.One / sum;
FPVector.Multiply(ref v01, b0, out point);
FPVector.Multiply(ref v11, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v21, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v31, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v02, b0, out temp2);
FPVector.Add(ref temp2, ref point, out point);
FPVector.Multiply(ref v12, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v22, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v32, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref point, inv * FP.Half, 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);
//FP d1 = JVector.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//FP d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
//UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0);
FPVector.Cross(ref v4, ref v0, out temp1);
//UnityEngine.Debug.LogError("temp1:"+ temp1);
//Ender
// UnityEngine.Debug.Log("normal: " + normal);
FP dot = FPVector.Dot(ref temp1, ref v1);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 );
dot = FPVector.Dot(ref temp1, ref v2);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v1->v4");
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < v3->v4");
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
// UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 );
dot = FPVector.Dot(ref temp1, ref v3);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v2 => v4");
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < 0 v1 => v4");
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref FPMatrix orientation1,
ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2,
out FPVector p1, out FPVector p2, out FPVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = FPVector.zero;
FPVector r = position1 - position2;
FPVector w, v;
FPVector supVertexA;
FPVector rn, vn;
rn = FPVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
FPVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = FPVector.zero;
int maxIter = MaxIterations;
FP distSq = v.sqrMagnitude;
FP epsilon = CollideEpsilon;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = FPVector.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.sqrMagnitude;
normal = v;
}
else
{
distSq = FP.Zero;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.sqrMagnitude > FPMath.Epsilon * FPMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}