/// Get the mass data of the body. The rotational inertia is relative /// to the center of mass. /// @return a struct containing the mass, inertia and center of the body. public void GetMassData(out MassData data) { data = new MassData(); data.Mass = _mass; data.I = _I; Vec2 center = Vec2.Zero; for (Fixture f = _fixtureList; f != null; f = f._next) { MassData massData = f.GetMassData(); _mass += massData.Mass; center += massData.Mass * massData.Center; _I += massData.I; } // Compute center of mass. if (_mass > 0.0f) { _invMass = 1.0f / _mass; center *= _invMass; } if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) { // Center the inertia about the center of mass. _I -= _mass * Vec2.Dot(center, center); Box2DXDebug.Assert(_I > 0.0f); _invI = 1.0f / _I; } else { _I = 0.0f; _invI = 0.0f; } // Move center of mass. Vec2 oldCenter = _sweep.C; _sweep.LocalCenter = center; _sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter); // Update center of mass velocity. _linearVelocity += Vec2.Cross(_angularVelocity, _sweep.C - oldCenter); BodyType oldType = _type; if (_invMass == 0.0f && _invI == 0.0f) { _type = BodyType.Static; _angularVelocity = 0.0f; _linearVelocity.SetZero(); } else { _type = BodyType.Dynamic; } // If the body type changed, we need to flag contacts for filtering. if (oldType != _type) { for (ContactEdge ce = _contactList; ce != null; ce = ce.Next) { ce.Contact.FlagForFiltering(); } } }
/// <summary> // From Real-time Collision Detection, p179. /// </summary> public void RayCast(out RayCastOutput output, RayCastInput input) { float tmin = -Common.Settings.FLT_MAX; float tmax = Common.Settings.FLT_MAX; output = new RayCastOutput(); output.Hit = false; Vec2 p = input.P1; Vec2 d = input.P2 - input.P1; Vec2 absD = Common.Math.Abs(d); Vec2 normal = new Vec2(0); for (int i = 0; i < 2; ++i) { if (absD[i] < Common.Settings.FLT_EPSILON) { // Parallel. if (p[i] < LowerBound[i] || UpperBound[i] < p[i]) { return; } } else { float inv_d = 1.0f / d[i]; float t1 = (LowerBound[i] - p[i]) * inv_d; float t2 = (UpperBound[i] - p[i]) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { Common.Math.Swap(ref t1, ref t2); s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); normal[i] = s; tmin = t1; } // Pull the max down tmax = Common.Math.Min(tmax, t2); if (tmin > tmax) { return; } } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.MaxFraction < tmin) { return; } // Intersection. output.Fraction = tmin; output.Normal = normal; output.Hit = true; }
public void RayCast(out RayCastOutput output, RayCastInput input) { output = new RayCastOutput(); float tmin = -Settings.FLT_MAX; float tmax = Settings.FLT_MAX; output.Hit = false; Vec2 p = input.P1; Vec2 d = input.P2 - input.P1; Vec2 absD = Math.Abs(d); Vec2 normal = new Vec2(); if (absD.X < Settings.FLT_EPSILON) { // Parallel. if (p.X < LowerBound.X || UpperBound.X < p.X) { return; } } else { float inv_d = 1.0f / d.X; float t1 = (LowerBound.X - p.X) * inv_d; float t2 = (UpperBound.X - p.X) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { Common.Math.Swap(ref t1, ref t2); s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); normal.X = s; tmin = t1; } // Pull the max down tmax = Common.Math.Min(tmax, t2); if (tmin > tmax) { return; } } if (absD.Y < Settings.FLT_EPSILON) { // Parallel. if (p.Y < LowerBound.Y || UpperBound.Y < p.Y) { return; } } else { float inv_d = 1.0f / d.Y; float t1 = (LowerBound.Y - p.Y) * inv_d; float t2 = (UpperBound.Y - p.Y) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { Common.Math.Swap(ref t1, ref t2); s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); normal.Y = s; tmin = t1; } // Pull the max down tmax = Common.Math.Min(tmax, t2); if (tmin > tmax) { return; } } //// Does the ray start inside the box? //// Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.MaxFraction < tmin) { return; } //// Intersection. output.Fraction = tmin; output.Normal = normal; output.Hit = true; }
/// <summary> /// Set this to the identity transform. /// </summary> public void SetIdentity() { Position.SetZero(); R.SetIdentity(); }