public Contact(RigidBody a, RigidBody b, Vector2 con, Vector2 axis, float disp)
{
A = a;
B = b;
ConPoint = con;
Disp = disp;
Axis = axis;
}
void Detonate(RigidBody sender, object ignoreParamater)
{
this.Kill();
PolyBody.ChangeWidth(BlastRange);
PolyBody.ChangeHieght(BlastRange);
var game = GameEngine.Singleton.GetPlayState();
foreach (Actor actor in game.ActiveMap.GameObjects)
{
if (actor != Launcher &&
CollisionEngine.TestCollisionPoly(this.PolyBody, actor.PolyBody))
actor.PolyBody.Gravity = WeaponPower*Vector2.UnitY;
}
}
public void OnCollided(RigidBody obj, Vector2 axis, float disp)
{
if (Collided != null) Collided(obj, new CollisionEventData(axis, disp));
if (!obj.IsSleeping && (obj.PureEnergy==2 || obj.PureEnergy==1 || this.GetCurrActivity() > SleepEpsilon || obj.GetCurrActivity() > SleepEpsilon)) //if (!obj.IsSleeping && ((this.GetCurrActivity() > SleepEpsilon) || (disp < -.0001f)) && (this.sleepID != obj.sleepID))
{
this.OMGWAKEUP();
}
else if (sleeping)
{
this.HaltThisIsThePolice();
}
switch (this.PureEnergy)
{
case 2:
_pureEnergy = 1;
break;
case 1:
if (obj.PureEnergy == 2) _pureEnergy = 2;
else _pureEnergy = 0;
break;
case 0:
if (obj.PureEnergy == 1 && !alreadyActivated)
_pureEnergy = 1;
else if (obj.PureEnergy == 2) _pureEnergy = 1;
break;
}
}
void PolyBody_Collided(RigidBody sender, object delta)
{
var colD = (CollisionEventData)delta;
var axis = colD.Axis;
if ((axis + Vector2.UnitY).LengthSquared() < 0.01f)
{
canChangeState = true;
if (changeToState.Name != "Empty")
{
State = changeToState;
FacingDirection = changeToDirection;
changeToState = new ActorState("Empty");
}
}
}
public static bool TestCollisionSpiritBody(RigidBody sp, RigidBody bod, out Vector2 axis, out float disp)
{
axis = new Vector2();
disp = float.MinValue;
if (sp is Particle && bod is PolyBody)
{
return TestCollisionPartiPoly((Particle)sp, (PolyBody)bod, out axis, out disp);
}
else
{
throw new NotImplementedException();
}
}
public void RemoveRigidBody(RigidBody b)
{
b.OnDeleted();
bodies.Remove(b);
}
public void AddRigidBody(RigidBody b)
{
AddRigidBody(b, true);
}
private void UpdateIter(float dt)
{
//var all = new List<RigidBody>();
// all.AddRange(bodies);
//all.AddRange(spirits);
RigidBody[] bTemp = new RigidBody[bodies.Count];
bodies.CopyTo(bTemp);
foreach (var b in bTemp)//add forces from force generators
{
if (!b.IsFixed) //don't update/check collisions w/ a fixed obj. as base.
{
if (!b.IsSleeping)
{
//
//advance the state
//
b.FGens.ForEach(x => b.AddForce(x(b))); //step 1: sum forces
Vector2 a = b.FNET * b.InvMass; //step 2: calc acceleration
b.Velocity += a * dt; //step 3: ???
b.Position += b.Velocity * dt; //step 4: PROFIT!!
b.TGens.ForEach(g => b.AddTorque(g(b)));
float ang_a = b.TNET * b.GetInverseInertiaMoment();
b.AngularVelocity += ang_a * dt;
b.Rotation += b.AngularVelocity * dt;
b.ResetForces();
}
b.UpdateActivity(dt);
if (b.IsSleepy())
b.GoToSleep();
}
}
RigidBody[] sTemp = new RigidBody[spirits.Count];
spirits.CopyTo(sTemp);
foreach (var s in sTemp)//add forces from force generators
{
//
//advance the state
//
var b = (Particle)s;
b.FGens.ForEach(x => b.AddForce(x(b))); //step 1: sum forces
Vector2 a = b.FNET * b.InvMass; //step 2: calc acceleration
b.Velocity += a * dt; //step 3: ???
b.Position += b.Velocity * dt; //step 4: PROFIT!!
b.TGens.ForEach(g => b.AddTorque(g(b)));
float ang_a = b.TNET * b.GetInverseInertiaMoment();
b.AngularVelocity += ang_a * dt;
b.Rotation += b.AngularVelocity * dt;
b.ResetForces();
b.UpdateActivity(dt);
if (b.IsOld())
{
this.RemoveSpirit(s);
}
}
CollisionEngine.SolveAllBodies(bodies);
CollisionEngine.SolveAllDoPenitSpirits(spirits, bodies);
CollisionEngine.ResolvePenetrationAll_Cheap(bodies, 2);
}
//axis must be from a->b!
public static List<Contact> FindSubContactsBody(RigidBody a, RigidBody b, Vector2 axis, float disp)
{
if (a is PolyBody && b is PolyBody)
{
return FindSubContactsPoly((PolyBody)a, (PolyBody)b, axis, disp);
}
else
{
throw new NotImplementedException();
}
}
public static List<Vector2> FindPenetratingPoints(RigidBody a, RigidBody b, Vector2 axis)
{
if (a is PolyBody && b is PolyBody)
{
/*var cons = new List<Vector2>();
foreach (var v in ((PolyBody)a).GetTransformedVertices())
{
if (TestPointInPoly((PolyBody)b, v))
cons.Add(v);
}
foreach (var v in ((PolyBody)b).GetTransformedVertices())
{
if (TestPointInPoly((PolyBody)a, v))
cons.Add(v);
}
return cons;*/
Vector2[] supA = GetSupports(((PolyBody)b), axis); //get supports. instead of iterating over ALL vertices, only iterate over 4 closest, based on sep axis
Vector2[] supB = GetSupports(((PolyBody)a), -axis);
var cons = new List<Vector2>();
for (int i = 0; i < supA.Length; i++)
for (int j = 0; j < supB.Length; j++)
{
Vector2 vout;
if (TestCollisionLine(supA[i], supA[(i + 1) % supA.Length], supB[j], supB[(j + 1) % supB.Length], out vout))
{
cons.Add(vout);
}
}
return cons;
}
else
{
throw new NotImplementedException();
}
}
public static void ApplyPenData(RigidBody body, ObjPenetrationData penD)
{
body.SetMasterPosition(body.Position + penD.dPos);
body.SetMasterRotation(body.Rotation + penD.dRot);
}
public static void ApplyColData(RigidBody body, ObjCollisionData colD)
{
body.SetMasterPosition(body.Position + colD.dPos);
body.Velocity += colD.dVel;
body.AngularVelocity += colD.dAngVel;
}
/// <summary>
/// generates a contact (and a list of subcontacts) for a pair of rigid bodies
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="con"></param>
/// <returns></returns>
private static bool testAndGenCon(RigidBody a, RigidBody b, out ContactP con)
{
Vector2 axis;
float disp;
con = new ContactP();
if (!(a.IsFixed && b.IsFixed) && TestCollisionBody(a, b, out axis, out disp))
{
var pcons = FindPenetratingPoints(a, b, axis);
con = new ContactP(a, b, axis, disp);
con.ConPoints = pcons;
return true;
}
return false;
}
//axis points a->b
private static void resolveSingleCon(RigidBody a, RigidBody b, Vector2 con, Vector2 axis, float disp)
{
/*float thresh = -.001f;
if (disp > thresh)
{
return;
}
else
{
disp -= thresh;
}*/
var Ia = a.GetInertiaMoment();
var Ib = b.GetInertiaMoment();
var Ia_inv = a.GetInverseInertiaMoment();
var Ib_inv = b.GetInverseInertiaMoment();
var n = -axis; //points from B->A!!
var n3 = n.ToVector3();
var relPosA = (con - a.Position).ToVector3();
var relPosB = (con - b.Position).ToVector3();
Vector3 velA, velB, tvelA, tvelB;
tvelA = Vector3.Cross(a.AngularVelocity * Vector3.UnitZ, relPosA); // tangentVel = (contactpt - centroid)x(angVel) // remember angVel is really on the Z axis.
tvelB = Vector3.Cross(b.AngularVelocity * Vector3.UnitZ, relPosB); //also, remember position is the position of the axis of rotation (centroid, or fixed point). which works our well.
velA = a.Velocity.ToVector3() + tvelA; //velocity of contact point = (vel of object) + (tangential vel of pt due to ang_vel)
velB = b.Velocity.ToVector3() + tvelB;
var torquePerImpulseA = Vector3.Cross(relPosA, n3);
var torquePerImpulseB = Vector3.Cross(relPosB, n3);
var rotPerImpA = torquePerImpulseA * Ia_inv;
var rotPerImpB = torquePerImpulseB * Ib_inv;
var velPerImpA = Vector3.Cross(rotPerImpA, relPosA); //angularInertiaWorld (pg326)
var velPerImpB = Vector3.Cross(rotPerImpB, relPosB);
Matrix localToWorld = CreateOrthonormalBasis(n, n.Perpen());
Matrix worldToLocal = Matrix.Transpose(localToWorld);
var angularInertiaA = velPerImpA.TransformToLocal(worldToLocal).X; //"deltaVelocity"
var angularInertiaB = velPerImpB.TransformToLocal(worldToLocal).X;
var linearInertiaA = Ia_inv;
var linearInertiaB = Ib_inv;
var totalInertia = angularInertiaA + angularInertiaB + linearInertiaA + linearInertiaB;
var invI = 1 / totalInertia;
var linearMoveA = disp * linearInertiaA * invI;
var linearMoveB = -disp * linearInertiaB * invI;
var angMoveA = disp * angularInertiaA * invI;
var angMoveB = -disp * angularInertiaB * invI;
var impulsiveTorqueA = Vector3.Cross(relPosA, n3);
var impulsiveTorqueB = Vector3.Cross(relPosB, n3);
var impPerMoveA = impulsiveTorqueA * Ia_inv;
var impPerMoveB = impulsiveTorqueB * Ib_inv;
a.Position += linearMoveA * axis;
b.Position += linearMoveB * axis;
a.Rotation += -impPerMoveA.Z * angMoveA * Ia_inv;
b.Rotation += -impPerMoveB.Z * angMoveB * Ib_inv;
}
void Collided(RigidBody sender, object ignoreParamater)
{
var game = GameEngine.Singleton.GetPlayState();
if (game.ActiveMap.MainPlayer.PolyBody == sender)
{
this.actUpon();
if (CollidedWith != null)
CollidedWith(this, (PolyBody)sender);
}
}
private void addDefaults(RigidBody b)
{
FieldInfo[] fis = typeof(DefaultGenerators).GetFields();
foreach (var fi in fis)
{
if (fi.FieldType == typeof(ForceGenerator))
{
b.FGens.Add((ForceGenerator)fi.GetValue(null));
}
else if (fi.FieldType == typeof(TorqueGenerator))
{
b.TGens.Add((TorqueGenerator)fi.GetValue(null));
}
}
}
public static void ResolvePenetration_Cheap(RigidBody a, RigidBody b, Vector2 axis, float disp)
{
ObjPenetrationData pA = new ObjPenetrationData();
ObjPenetrationData pB = new ObjPenetrationData();
var totInvMass = a.InvMass + b.InvMass;
pA.dPos = +disp * axis * a.InvMass / totInvMass;
pB.dPos = -disp * axis * b.InvMass / totInvMass;
pA.dRot = 0f;
pB.dRot = 0f;
a.PenData.Add(pA);
b.PenData.Add(pB);
}
public void AddRigidBody(RigidBody b, bool addDefaultGenerators)
{
bodies.Add(b);
if (addDefaultGenerators) addDefaults(b);
}
public static bool TestCollisionBody(RigidBody a, RigidBody b, out Vector2 axis, out float disp)
{
axis = new Vector2();
disp = float.MinValue;
if (a is PolyBody && b is PolyBody)
{
return TestCollisionPoly((PolyBody)a, (PolyBody)b, out axis, out disp);
}
else
{
throw new NotImplementedException();
}
}
public void AddSpirit(RigidBody b)
{
spirits.Add(b);
addDefaults(b);
}
public ContactP(RigidBody a, RigidBody b, Vector2 axis, float disp)
{
A = a;
B = b;
ConPoints = new List<Vector2>();
Disp = disp;
Axis = axis;
}
public void RemoveSpirit(RigidBody s)
{
s.OnDeleted();
spirits.Remove(s);
}
void p_Deleted(RigidBody sender, object delta)
{
this.Kill();
}