public static bool boxAndSphere(
BoxRigid box,
SphereRigid sphere,
ref CollisionData data
)
{
// Transform the centre of the sphere into box coordinates
Vector3 centre = sphere.PositionCenterEngine;
Vector3 relCentre = centre - box.PositionCenterEngine; //box.transform.transformInverse(centre);
relCentre = Matrix2.M_V(relCentre, -box.GetOrientation());
// Early out check to see if we can exclude the contact
if (Math.Abs(relCentre.X) - sphere.Radius > box.HalfSize.X ||
Math.Abs(relCentre.Y) - sphere.Radius > box.HalfSize.Y
)
{
return(false);
}
Vector3 closestPt = new Vector3(0, 0, 0);
float dist;
// Clamp each coordinate to the box.
dist = relCentre.X;
if (dist > box.HalfSize.X)
{
dist = box.HalfSize.X;
}
if (dist < -box.HalfSize.X)
{
dist = -box.HalfSize.X;
}
closestPt.X = dist;
dist = relCentre.Y;
if (dist > box.HalfSize.Y)
{
dist = box.HalfSize.Y;
}
if (dist < -box.HalfSize.Y)
{
dist = -box.HalfSize.Y;
}
closestPt.Y = dist;
// Check we're in contact
Vector3 temp1 = closestPt - relCentre;
if (temp1 == Vector3.Zero)
{
return(true);
}
float temp2 = temp1.Length();
double temp3 = temp2;
dist = (float)Math.Pow(temp3, 2);
if (dist > sphere.Radius * sphere.Radius)
{
return(false);
}
// Compile the contact
Vector3 closestPtWorld = closestPt;//box.transform.transform(closestPt);
closestPtWorld = Matrix2.M_V(closestPtWorld, box.GetOrientation());
closestPtWorld += box.PositionCenterEngine;
//Contact* contact = data->contacts;
// Create the contact data
Vector3 temp = closestPtWorld - centre;
temp.Normalize();
Contact c = new Contact();
c.ContactNormal = temp;
c.Penetration = (float)(sphere.Radius - Math.Sqrt(dist));
c.ContactPoint = closestPtWorld;
c.SetBodyData(box, sphere,
data.friction, data.restitution);
c.Friction = StaticData.FrictionTable[(int)sphere.GetMaterial()][(int)box.GetMaterial()];
// between bump and cookie
c.Restitution = 1f; // StaticData.RestitutionTable[(int)sphere.GetMaterial()][(int)box.GetMaterial()];
c.ContactToWorld.M11 = c.ContactNormal.X;
c.ContactToWorld.M12 = -c.ContactNormal.Y;
c.ContactToWorld.M21 = c.ContactNormal.Y;
c.ContactToWorld.M22 = c.ContactNormal.X;
data.addContacts(1);
data.contacts.Add(c);
return(true);
}
static void fillPointFaceBoxBox(
BoxRigid one,
BoxRigid two,
Vector3 toCentre,
ref CollisionData data,
int best,
float pen
)
{
// This method is called when we know that a vertex from
// box two is in contact with box one.
// Contact contact = data.contacts;
// We know which axis the collision is on (i.e. best),
// but we need to work out which of the two faces on
// this axis.
Vector3 normal;
if (best == 0)
{
normal = ((BoxRigid)one).XAxis;
}
else
{
normal = ((BoxRigid)one).YAxis;
}
if (Vector3.Dot(normal, toCentre) > 0) /*one.getAxis(best) * toCentre*/
{
normal = normal * -1.0f;
}
// Work out which vertex of box two we're colliding with.
// Using toCentre doesn't work!
Vector3 vertex = two.HalfSize;
if (Vector3.Dot(((BoxRigid)two).XAxis, normal) < 0)
{
vertex.X = -vertex.X;
}
if (Vector3.Dot(((BoxRigid)two).YAxis, normal) < 0)
{
vertex.Y = -vertex.Y;
}
vertex = Matrix2.M_V(vertex, two.GetOrientation());
vertex += two.PositionCenterEngine;
Contact c = new Contact();
c.ContactToWorld.M11 = normal.X;
c.ContactToWorld.M12 = -normal.Y;
c.ContactToWorld.M21 = normal.Y;
c.ContactToWorld.M22 = normal.X;
// Create the contact data
c.ContactNormal = normal;
c.Penetration = pen;
c.ContactPoint = vertex; //Matrix2.transform(data.contacts[data.index].contactToWorld, vertex); /////////////////// مشكوكة
c.SetBodyData(one, two,
data.friction, data.restitution);
data.contacts.Add(c);
}