static bool tryAxis(
BoxRigid one,
BoxRigid two,
Vector3 axis,
Vector3 toCentre,
int index,
// These values may be updated
ref float smallestPenetration,
ref int smallestCase
)
{
// Make sure we have a normalized axis, and don't check almost parallel axes
if (axis.Length() * axis.Length() < 0.00001)
{
return(true);
}
axis.Normalize();
float penetration = penetrationOnAxis(one, two, axis, toCentre);
if (penetration < 0)
{
return(false);
}
if (penetration < smallestPenetration)
{
smallestPenetration = penetration;
smallestCase = index;
}
return(true);
}
static float transformToAxis(
BoxRigid box,
Vector3 axis
)
{
return
(box.HalfSize.X * (float)Math.Abs(Vector3.Dot(axis, ((BoxRigid)box).XAxis)) +
box.HalfSize.Y * (float)Math.Abs(Vector3.Dot(axis, ((BoxRigid)box).YAxis)));
}
public BoxRigid AddBoxRigidToEngine()
{
BoxRigid boxRigidCreated = DefaultAdder.GetDefaultBox(
new Vector3(mouseState.X - 3, (mouseState.Y - 3), 0),
Material.Steel,
StaticData.BoxDefaultSize,
null,
null,
null,
0,
false);
StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids.Add(boxRigidCreated);
return(boxRigidCreated);
}
public static string GetAllBumpsPositionsOnly()
{
String allServicesString = String.Empty;
for (int i = 0; i < StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids.Count; i++)
{
String currentString = String.Empty;
BoxRigid boxRigid = StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids[i];
if (boxRigid is BumpRigid)
{
currentString = "bump(" + boxRigid.PositionXNA.X + ", " + boxRigid.PositionXNA.Y + ").";
allServicesString += currentString + Environment.NewLine;
}
}
return(allServicesString);
}
private void ManipulateAddingBoxes()
{
if (mouseState.LeftButton == ButtonState.Pressed)
{
if (_boxRigidAdded == null)
{
_boxRigidAdded = AddBoxRigidToEngine();
}
}
else
{
if (_boxRigidAdded != null && mouseState.LeftButton == ButtonState.Released)
{
_boxRigidAdded = null;
}
}
}
public static BoxRigid GetDefaultBox(
Vector3 positionXNA,
Material material,
Vector3 halfSize,
Vector3?acc,
Vector3?initialForce,
Vector3?initialTorque,
float orientationValue = 0,
bool obInertia = false)
{
BoxRigid rectToReturn = new BoxRigid(positionXNA,
material,
new Vector3(halfSize.X, halfSize.Y, 0));
// Inertia
if (obInertia)
{
rectToReturn.SetObInertia();
// Awaking
rectToReturn.SetAwake(true);
return(rectToReturn);
}
// Acc
Vector3 accVector = acc ?? new Vector3(0, -StaticData.GravityConstant, 0);
rectToReturn.SetAcceleration(accVector);
// Force
Vector3 forceIn = initialForce ?? new Vector3(0, 0, 0);
rectToReturn.AddForce(forceIn);
// Torque
Vector3 torqueIn = initialTorque ?? new Vector3(0, 0, 0);
rectToReturn.AddTorque(torqueIn, MathHelperModule.GetInverseYPosition(rectToReturn.PositionCenterEngine));
// Orientation
rectToReturn.SetOrientation(orientationValue);
// Awaking
rectToReturn.SetAwake(true);
return(rectToReturn);
}
static bool IntersectionTestboxAndHalfSpace(
BoxRigid box,
CollisionPlane plane
)
{
// Work out the projected radius of the box onto the plane direction
float projectedRadius = transformToAxis(box, plane.Direction);
// Work out how far the box is from the origin
float boxDistance =
Vector3.Dot(plane.Direction,
box.PositionCenterEngine) -
projectedRadius;
// Check for the intersection
return(boxDistance <= plane.Offset);
}
public static bool overlapOnAxis(
BoxRigid one,
BoxRigid two,
Vector3 axis,
Vector3 toCentre
)
{
// Project the half-size of one onto axis
float oneProject = transformToAxis(one, axis);
float twoProject = transformToAxis(two, axis);
// Project this onto the axis
float distance = Math.Abs(Vector3.Dot(toCentre, axis));
// Check for overlap
return(distance < oneProject + twoProject);
}
/*
* This function checks if the two boxes overlap
* along the given axis, returning the ammount of overlap.
* The final parameter toCentre
* is used to pass in the vector between the boxes centre
* points, to avoid having to recalculate it each time.
*/
static float penetrationOnAxis(
BoxRigid one,
BoxRigid two,
Vector3 axis,
Vector3 toCentre
)
{
// Project the half-size of one onto axis
float oneProject = transformToAxis(one, axis);
float twoProject = transformToAxis(two, axis);
// Project this onto the axis
float distance = Math.Abs(Vector3.Dot(toCentre, axis));
// Return the overlap (i.e. positive indicates
// overlap, negative indicates separation).
return(oneProject + twoProject - distance);
}
public static bool IntersectionTestsboxAndBox(
BoxRigid one,
BoxRigid two
)
{
// Find the vector between the two centres
Vector3 toCentre = two.PositionCenterEngine - one.PositionCenterEngine;
return(
// Check on box one's axes first
overlapOnAxis(one, two, ((BoxRigid)one).XAxis, toCentre) &&
overlapOnAxis(one, two, ((BoxRigid)one).YAxis, toCentre) &&
// And on two's
overlapOnAxis(one, two, ((BoxRigid)two).XAxis, toCentre) &&
overlapOnAxis(one, two, ((BoxRigid)two).YAxis, toCentre) //&&
);
}
public static string GetAllBumpsGEVA(List <Visual2D> comps)
{
String allServicesString = String.Empty;
for (int i = 0; i < StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids.Count; i++)
{
String currentString = String.Empty;
BoxRigid boxRigid = StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids[i];
if (boxRigid is BumpRigid)
{
if (comps.Contains(boxRigid))
{
currentString = "bump(" + boxRigid.PositionXNA.X + ", " + boxRigid.PositionXNA.Y +
", " + (int)((BumpRigid)boxRigid).Dir + ").";
allServicesString += currentString + Environment.NewLine;
}
}
}
return(allServicesString);
}
public static string GetAllBumpsProlog(List <Point> listOfInterest)
{
String allServicesString = String.Empty;
for (int i = 0; i < StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids.Count; i++)
{
String currentString = String.Empty;
BoxRigid boxRigid = StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids[i];
if (boxRigid is BumpRigid)
{
if (IsInList(listOfInterest, boxRigid.PositionXNACenter.X, boxRigid.PositionXNACenter.Y))
{
currentString = "bump(" + boxRigid.PositionXNACenter.X + ", " + boxRigid.PositionXNACenter.Y +
", " + (int)((BumpRigid)boxRigid).Dir + ").";
allServicesString += currentString + Environment.NewLine;
}
}
}
return(allServicesString);
}
private void BoxesCD()
{
List <BoxRigid> ListOfBoxRigids = StaticData.EngineManager.RigidsManagerEngine.ListOfBoxRigids;
List <CollisionPlane> ListOfPlanes = null;
if (RyseAgent.WithWalls)
{
ListOfPlanes = StaticData.EngineManager.PlanesManagerEngine.ListOfPlanes;
}
for (int i = 0; i < ListOfBoxRigids.Count; i++)
{
// Cash the box
BoxRigid box = ListOfBoxRigids[i];
// boxAndHalfSpace
if (RyseAgent.WithWalls)
{
foreach (var plane in ListOfPlanes)
{
CollisionDetector.boxAndHalfSpace(box, plane, ref _data);
}
}
if (box.IsCollidable)
{
// boxAndBox
for (int j = 0; j < ListOfBoxRigids.Count; j++)
{
BoxRigid box2 = ListOfBoxRigids[j];
if (box != box2)
{
if (box2.IsCollidable)
{
if (!IsPairOfNonCollidableRigids(box, box2))
{
CollisionDetector.boxAndBox(box, box2, ref _data);
}
}
}
}
}
}
}
public static bool boxAndBox(
BoxRigid one,
BoxRigid two,
ref CollisionData data
)
{
if (!IntersectionTestsboxAndBox(one, two))
{
return(false);
}
// Find the vector between the two centres
Vector3 toCentre = two.PositionCenterEngine - one.PositionCenterEngine;
// We start assuming there is no contact
float pen = float.MaxValue;
int best = 0xffffff;
// Now we check each axes, returning if it gives us
// a separating axis, and keeping track of the axis with
// the smallest penetration otherwise.
if (!tryAxis(one, two, (one).XAxis, toCentre, 0, ref pen, ref best))
{
return(false);
}
if (!tryAxis(one, two, (one).YAxis, toCentre, 1, ref pen, ref best))
{
return(false);
}
if (!tryAxis(one, two, (two).XAxis, toCentre, 2, ref pen, ref best))
{
return(false);
}
if (!tryAxis(one, two, (two).YAxis, toCentre, 3, ref pen, ref best))
{
return(false);
}
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
int bestSingleAxis = best;
// Make sure we've got a result.
if (best != 0xffffff)
{
// We now know there's a collision, and we know which
// of the axes gave the smallest penetration. We now
// can deal with it in different ways depending on
// the case.
if (best < 2)
{
// We've got a vertex of box two on a face of box one.
fillPointFaceBoxBox(one, two, toCentre, ref data, best, pen);
one.SetCanSleep(true);
two.SetCanSleep(true);
data.addContacts(1);
}
else if (best < 4)
{
fillPointFaceBoxBox(two, one, toCentre * -1.0f, ref data, best - 2, pen);
one.SetCanSleep(true);
two.SetCanSleep(true);
data.addContacts(1);
}
return(true);
}
return(false);
}
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);
}
public static bool boxAndHalfSpace(
BoxRigid box,
CollisionPlane plane,
ref CollisionData data
)
{
// Make sure we have contacts
if (data.contactsLeft <= 0)
{
return(false);
}
// Check for intersection
if (!IntersectionTestboxAndHalfSpace(box, plane))
{
return(false);
}
bool b = false;
int contactsUsed = 0;
for (int i = 0; i < 4; i++)
{
Vector3 vertexPos = ((BoxRigid)box).vertices[i].Position;
// Calculate the distance from the plane
float vertexDistance = Vector3.Dot(vertexPos, plane.Direction);
// Compare this to the plane's distance
if (vertexDistance <= plane.Offset)
{
b = true;
// Create the contact data.
// The contact point is halfway between the vertex and the
// plane - we multiply the direction by half the separation
// distance and add the vertex location.
Contact c = new Contact();
c.ContactPoint = plane.Direction;
c.ContactPoint *= (0.5f * (plane.Offset - vertexDistance));
c.ContactPoint = vertexPos;
c.ContactNormal = plane.Direction;
c.Penetration = plane.Offset - vertexDistance;
c.Particle[0] = box;
c.Particle[1] = null;
c.Friction = data.friction;
c.Restitution = data.restitution;
c.Friction = StaticData.FrictionTable[(int)plane.Material][(int)box.GetMaterial()];
c.Restitution = StaticData.RestitutionTable[(int)plane.Material][(int)box.GetMaterial()];
c.ContactToWorld.M11 = plane.Direction.X;
c.ContactToWorld.M12 = -plane.Direction.Y;
c.ContactToWorld.M21 = plane.Direction.Y;
c.ContactToWorld.M22 = plane.Direction.X;
data.contacts.Add(c);
box.SetCanSleep(true);
data.index++;
data.contactsLeft--;
data.contactCount++;
contactsUsed++;
}
}
return(b);
}
private void ReIntitializeTag4Rectangle(Vector3 positionCenter, float distanceX, BoxRigid rectIn)
{
(rectIn).ReInitializeData
(
MathHelperModule.GetPositionXNA(positionCenter, distanceX / 2, rectIn.GetHalfHeight()),
rectIn.GetMaterial(),
new Vector3(distanceX / 2, rectIn.GetHalfHeight(), 0)
);
}
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);
}
public void MakeRod(RigidBody r1, RigidBody r2, float?spacing, float rodLinkHeight, float forgivingFactor)
{
BoxRigidHardConstraint boxLink;
if (spacing == null)
{
boxLink = DefaultAdder.GetDefaultBoxRigidHardConstraint(r1.PositionXNA, Material.Wood,
new Vector3((r1.PositionXNA - r2.PositionXNA).Length(),
rodLinkHeight,
0),
new Vector3(0, 0, 0),
null,
null, 0, false);
}
else
{
boxLink = DefaultAdder.GetDefaultBoxRigidHardConstraint(r1.PositionXNA, Material.Wood,
new Vector3((float)spacing, rodLinkHeight, 0),
new Vector3(0, 0, 0),
null,
null, 0, false);
}
//boxLink.SetMass(100000);
//r1.SetMass(50);
//r2.SetMass(50);
//boxLink.IsCollidable = false;
boxLink.IsDrawable = false;
int VertexOneIndex = -1;
int VertexTwoIndex = -1;
this.RigidOne = r1;
this.RigidTwo = r2;
this.VertexOneIndex = VertexOneIndex;
this.VertexTwoIndex = VertexTwoIndex;
this.RodRigidBody = boxLink;
this.Joint1 = new RodJoint(forgivingFactor);
this.Joint2 = new RodJoint(forgivingFactor);
Vector3 position1, position2 = new Vector3();
if (VertexOneIndex == -1)
{
position1 = r1.PositionCenterEngine;
}
else
{
position1 = r1.vertices[VertexOneIndex].Position;
}
if (VertexTwoIndex == -1)
{
position2 = r2.PositionCenterEngine;
}
else
{
position2 = r2.vertices[VertexTwoIndex].Position;
}
int vertexBoxOneIndex = boxLink.FindJointIndex(position1);
int vertexBoxTwoIndex = boxLink.FindJointIndex(position2);
this.VertexBoxOneIndex = vertexBoxOneIndex;
this.VertexBoxTwoIndex = vertexBoxTwoIndex;
this.Joint1.Set(r1, position1, boxLink, boxLink.vertices[vertexBoxOneIndex].Position);
this.Joint2.Set(r2, position2, boxLink, boxLink.vertices[vertexBoxTwoIndex].Position);
}
private void ReIntitializeTag3Rectangle(Vector3 positionCenter, float distanceY, BoxRigid rectIn)
{
(rectIn).ReInitializeData
(
MathHelperModule.GetPositionXNA(positionCenter, rectIn.GetHalfWidth(), distanceY / 2),
rectIn.GetMaterial(),
new Vector3(rectIn.GetHalfWidth(), distanceY / 2, 0)
);
}
