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); }