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