internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocity;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= InvIA * impulse;
wB += InvIB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = _mass * (-Cdot);
_impulse += impulse;
Vector2 P = impulse * _ay;
float LA = impulse * _sAy;
float LB = impulse * _sBy;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return(false);
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -normal;
}
else
{
output.Normal = normal;
}
return(true);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
LocalCenterA = bA.LocalCenter;
LocalCenterB = bB.LocalCenter;
Transform xfA;
bA.GetTransform(out xfA);
Transform xfB;
bB.GetTransform(out xfB);
// Compute the effective masses.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - LocalCenterA);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + rB - bA.Sweep.C - rA;
InvMassA = bA.InvMass;
InvIA = bA.InvI;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Point to line constraint
{
_ay = MathUtils.Multiply(ref xfA.R, _localYAxisA);
_sAy = MathUtils.Cross(d + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
}
// Spring constraint
_springMass = 0.0f;
if (Frequency > 0.0f)
{
_ax = MathUtils.Multiply(ref xfA.R, LocalXAxis);
_sAx = MathUtils.Cross(d + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
if (invMass > 0.0f)
{
_springMass = 1.0f / invMass;
float C = Vector2.Dot(d, _ax);
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float da = 2.0f * _springMass * DampingRatio * omega;
// Spring stiffness
float k = _springMass * omega * omega;
// magic formulas
_gamma = step.dt * (da + step.dt * k);
if (_gamma > 0.0f)
{
_gamma = 1.0f / _gamma;
}
_bias = C * step.dt * k * _gamma;
_springMass = invMass + _gamma;
if (_springMass > 0.0f)
{
_springMass = 1.0f / _springMass;
}
}
}
else
{
_springImpulse = 0.0f;
_springMass = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_motorMass = InvIA + InvIB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
else
{
_motorMass = 0.0f;
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_springImpulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse * _ay + _springImpulse * _ax;
float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
bA.LinearVelocityInternal -= InvMassA * P;
bA.AngularVelocityInternal -= InvIA * LA;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * LB;
}
else
{
_impulse = 0.0f;
_springImpulse = 0.0f;
_motorImpulse = 0.0f;
}
}
public bool AddCollision(Collision col)
{
/*if (collisions.ContainsKey(col.other))
{
Collision oldCol = collisions [col.other];
List<Vector2> depenetrations = new List<Vector2> ();
List<Vector2> contactPoints = new List<Vector2> ();
List<Vector2> normals = new List<Vector2> ();
List<float> ts = new List<float> ();
int ol = oldCol.depenetrations.Length;
int ol1 = ol - 1;
for (int i = 0; i < ol; ++i) {
Vector2 vd = oldCol.depenetrations [i];
depenetrations.Add (vd);
Vector2 vcp = oldCol.contactPoints [i];
contactPoints.Add (vcp);
Vector2 vn = oldCol.normals [i];
normals.Add (vn);
float t = oldCol.ts [i];
ts.Add (t);
}
int l = col.depenetrations.Length;
bool pass = false;
for (int i = 0; i < l; ++i) {
if (!pass && col.contactPoints[i] == oldCol.contactPoints[ol1]) {
continue;
}
pass = true;
Vector2 vd = col.depenetrations [i];
depenetrations.Add (vd);
Vector2 vcp = col.contactPoints [i];
contactPoints.Add (vcp);
Vector2 vn = col.normals [i];
normals.Add (vn);
float t = col.ts [i];
ts.Add (t);
}
Collision newCol = new Collision(
col.self,
col.other,
contactPoints.ToArray(),
depenetrations.ToArray(),
normals.ToArray(),
ts.ToArray()
);
if (oldCol == newCol) {
return false;
} else {
collisions [col.other] = newCol;
//ImmediateCollisionBroadcast (newCol);
return true;
}
}
else
{*/
collisions [col.other] = col;
if (rigidbody != null) {
int l = col.ts.Length;
Collision source = col;
for (int k = 0; k < l; ++k) {
Vector2 selfDepNorm = source.normals [k];
if (Vector2.Dot (selfDepNorm, source.self.physics.negativeNormalizedGravity) >= Rigidbody.groundMinimumCos) {
rigidbody.isGroundedGravity = true;
}
if (Vector2.Dot (selfDepNorm, source.self.transform.down) >= Rigidbody.groundMinimumCos) {
rigidbody.isGroundedUp = true;
}
if (Vector2.Dot (selfDepNorm, source.self.transform.up) >= Rigidbody.groundMinimumCos) {
rigidbody.isGroundedDown = true;
}
if (Vector2.Dot (selfDepNorm, source.self.transform.right) >= Rigidbody.groundMinimumCos) {
rigidbody.isGroundedLeft = true;
}
if (Vector2.Dot (selfDepNorm, source.self.transform.left) >= Rigidbody.groundMinimumCos) {
rigidbody.isGroundedRight = true;
}
}
}
return true;
}
public static bool GeneratePathedStairsAsset(CSGBrushMeshAsset brushMeshAsset, CSGPathedStairsDefinition definition)
{
definition.Validate();
var shapeVertices = new List <Vector2>();
var shapeSegmentIndices = new List <int>();
GetPathVertices(definition.shape, definition.curveSegments, shapeVertices, shapeSegmentIndices);
var totalSubMeshCount = 0;
for (int i = 0; i < shapeVertices.Count; i++)
{
if (i == 0 && !definition.shape.closed)
{
continue;
}
var leftSide = (!definition.shape.closed && i == 1) ? definition.stairs.leftSide : StairsSideType.None;
var rightSide = (!definition.shape.closed && i == shapeVertices.Count - 1) ? definition.stairs.rightSide : StairsSideType.None;
totalSubMeshCount += GetLinearStairsSubMeshCount(definition.stairs, leftSide, rightSide);
}
if (totalSubMeshCount == 0)
{
brushMeshAsset.Clear();
return(false);
}
// var stairDirections = definition.shape.closed ? shapeVertices.Count : (shapeVertices.Count - 1);
// TODO: use list instead?
CSGBrushSubMesh[] subMeshes;
if (brushMeshAsset.SubMeshCount != totalSubMeshCount)
{
subMeshes = new CSGBrushSubMesh[totalSubMeshCount];
for (int i = 0; i < totalSubMeshCount; i++)
{
subMeshes[i] = new CSGBrushSubMesh();
}
}
else
{
subMeshes = brushMeshAsset.SubMeshes;
}
var depth = definition.stairs.depth;
var height = definition.stairs.height;
var halfDepth = depth * 0.5f;
var halfHeight = height * 0.5f;
int subMeshIndex = 0;
for (int vi0 = shapeVertices.Count - 3, vi1 = shapeVertices.Count - 2, vi2 = shapeVertices.Count - 1, vi3 = 0; vi3 < shapeVertices.Count; vi0 = vi1, vi1 = vi2, vi2 = vi3, vi3++)
{
if (vi2 == 0 && !definition.shape.closed)
{
continue;
}
// TODO: optimize this, we're probably redoing a lot of stuff for every iteration
var v0 = shapeVertices[vi0];
var v1 = shapeVertices[vi1];
var v2 = shapeVertices[vi2];
var v3 = shapeVertices[vi3];
var m0 = (v0 + v1) * 0.5f;
var m1 = (v1 + v2) * 0.5f;
var m2 = (v2 + v3) * 0.5f;
var d0 = (v1 - v0);
var d1 = (v2 - v1);
var d2 = (v3 - v2);
var maxWidth0 = d0.magnitude;
var maxWidth1 = d1.magnitude;
var maxWidth2 = d2.magnitude;
var halfWidth1 = d1 * 0.5f;
d0 /= maxWidth0;
d1 /= maxWidth1;
d2 /= maxWidth2;
var depthVector = new Vector3(d1.y, 0, -d1.x);
var lineCenter = new Vector3(m1.x, halfHeight, m1.y) - (depthVector * halfDepth);
var depthVector0 = new Vector2(d0.y, -d0.x) * depth;
var depthVector1 = new Vector2(d1.y, -d1.x) * depth;
var depthVector2 = new Vector2(d2.y, -d2.x) * depth;
m0 -= depthVector0;
m1 -= depthVector1;
m2 -= depthVector2;
Vector2 output;
var leftShear = Intersect(m1, d1, m0, d0, out output) ? Vector2.Dot(d1, (output - (m1 - halfWidth1))) : 0;
var rightShear = Intersect(m1, d1, m2, d2, out output) ? -Vector2.Dot(d1, (output - (m1 + halfWidth1))) : 0;
var transform = Matrix4x4.TRS(lineCenter, // move to center of line
Quaternion.LookRotation(depthVector, Vector3.up), // rotate to align with line
Vector3.one);
// set the width to the width of the line
definition.stairs.width = maxWidth1;
definition.stairs.nosingWidth = 0;
var leftSide = (!definition.shape.closed && vi2 == 1) ? definition.stairs.leftSide : StairsSideType.None;
var rightSide = (!definition.shape.closed && vi2 == shapeVertices.Count - 1) ? definition.stairs.rightSide : StairsSideType.None;
var subMeshCount = GetLinearStairsSubMeshCount(definition.stairs, leftSide, rightSide);
if (subMeshCount == 0)
{
continue;
}
if (!GenerateLinearStairsSubMeshes(subMeshes, definition.stairs, leftSide, rightSide, subMeshIndex))
{
brushMeshAsset.Clear();
return(false);
}
var halfWidth = maxWidth1 * 0.5f;
for (int m = 0; m < subMeshCount; m++)
{
var vertices = subMeshes[subMeshIndex + m].Vertices;
for (int v = 0; v < vertices.Length; v++)
{
// TODO: is it possible to put all of this in a single matrix?
// lerp the stairs to go from less wide to wider depending on the depth of the vertex
var depthFactor = 1.0f - ((vertices[v].z / definition.stairs.depth) + 0.5f);
var wideFactor = (vertices[v].x / halfWidth) + 0.5f;
var scale = (vertices[v].x / halfWidth);
// lerp the stairs width depending on if it's on the left or right side of the stairs
vertices[v].x = Mathf.Lerp(scale * (halfWidth - (rightShear * depthFactor)),
scale * (halfWidth - (leftShear * depthFactor)),
wideFactor);
vertices[v] = transform.MultiplyPoint(vertices[v]);
}
}
subMeshIndex += subMeshCount;
}
brushMeshAsset.SubMeshes = subMeshes;
brushMeshAsset.CalculatePlanes();
brushMeshAsset.SetDirty();
return(false);
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
Vector2 r = p2 - p1;
Debug.Assert(r.sqrMagnitude > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 absV = MathUtils.Abs(new Vector2(-r.y, r.x));
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
VectorMath.Min(ref p1, ref t, out segmentAABB.LowerBound);
VectorMath.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_stack.Clear();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode <T> node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.AABB.Center;
Vector2 h = node.AABB.Extents;
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.y, r.x), p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Vector2.Min(p1, t);
segmentAABB.UpperBound = Vector2.Max(p1, t);
}
}
else
{
_stack.Push(node.Child1);
_stack.Push(node.Child2);
}
}
}
internal override bool SolvePositionConstraints()
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = b1.Sweep.C;
float a1 = b1.Sweep.A;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle);
linearError = Math.Max(linearError, Math.Abs(C1.x));
float angularError = Math.Abs(C1.y);
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3(-C1.x, -C1.y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.x;
impulse.Y = impulse1.y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= InvMassA * P;
a1 -= InvIA * L1;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b1.Sweep.C = c1;
b1.Sweep.A = a1;
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
public CollisionApplication(CollisionChild c)
{
source = c;
Vector2 velocity = c.self.velocity;
movementChange = Vector2.zero;
velocityChange = Vector2.zero;
isGroundedGravity = false;
isGroundedUp = false;
isGroundedDown = false;
isGroundedLeft = false;
isGroundedRight = false;
applicable = c.self.rigidbody != null; // && c.depenetration != Vector2.zero;
if (!applicable)
{
return;
}
movementChange = c.depenetration - c.movement;
positionChange = c.movement;
/*int l = source.normals.Length;
* for(int k = 0; k < l; ++k){*/
Vector2 norm = source.normal.normalized;
float selfVelocityNormalDot = Vector2.Dot(velocity.normalized, norm);
if (selfVelocityNormalDot < 0)
{
Vector2 a = Util.Project(velocity, norm);
Vector2 b = Util.Project(source.other.velocity, norm);
if (Vector2.Dot(a, b) > 0)
{
if (a.sqrMagnitude > b.sqrMagnitude)
{
velocityChange -= a - b;
}
}
else
{
velocityChange -= a;
}
}
//}
Rigidbody rb = source.self.rigidbody;
velocityChange = Util.Clamp(rb._velocity + velocityChange, rb._velocity, Vector2.zero) - rb._velocity;
//}
//for(int k = 0; k < l; ++k){
if (source.self.gameObject.name == "GreenBox" && source.other.gameObject.name == "WhiteBox")
{
//if (norm.y > 0) {
UnityEngine.Debug.Log("NORM " + Util.ToString(norm));
//}
}
Vector2 selfDepNorm = norm;
if (Vector2.Dot(selfDepNorm, source.self.physics.negativeNormalizedGravity) >= Rigidbody.groundMinimumCos)
{
isGroundedGravity = true;
}
if (Vector2.Dot(selfDepNorm, source.self.transform.down) >= Rigidbody.groundMinimumCos)
{
isGroundedUp = true;
}
if (Vector2.Dot(selfDepNorm, source.self.transform.up) >= Rigidbody.groundMinimumCos)
{
isGroundedDown = true;
}
if (Vector2.Dot(selfDepNorm, source.self.transform.right) >= Rigidbody.groundMinimumCos)
{
isGroundedLeft = true;
}
if (Vector2.Dot(selfDepNorm, source.self.transform.left) >= Rigidbody.groundMinimumCos)
{
isGroundedRight = true;
}
//}
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
// FPE 3 only - Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.x += step.dt * (b.InvMass * b.Force.x);
b.LinearVelocityInternal.y += step.dt * (b.InvMass * b.Force.y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
else
{
b.LinearVelocityInternal.x += step.dt * (gravity.x + b.InvMass * b.Force.x);
b.LinearVelocityInternal.y += step.dt * (gravity.y + b.InvMass * b.Force.y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < ContactCount; ++i2)
{
Fixture fixtureA = _contacts[i2].FixtureA;
Fixture fixtureB = _contacts[i2].FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
if (nonStatic)
{
++i1;
//TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
Contact tmp = _contacts[i1];
_contacts[i1] = _contacts[i2];
_contacts[i2] = tmp;
}
}
// Initialize velocity constraints.
_contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref step);
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref step);
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
_contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
_contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationX = step.dt * b.LinearVelocityInternal.x;
float translationY = step.dt * b.LinearVelocityInternal.y;
float result = translationX * translationX + translationY * translationY;
if (result > Settings.MaxTranslationSquared)
{
float sq = (float)Math.Sqrt(result);
float ratio = Settings.MaxTranslation / sq;
b.LinearVelocityInternal.x *= ratio;
b.LinearVelocityInternal.y *= ratio;
}
float rotation = step.dt * b.AngularVelocityInternal;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
b.AngularVelocityInternal *= ratio;
}
// Store positions for continuous collision.
b.Sweep.C0.x = b.Sweep.C.x;
b.Sweep.C0.y = b.Sweep.C.y;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C.x += step.dt * b.LinearVelocityInternal.x;
b.Sweep.C.y += step.dt * b.LinearVelocityInternal.y;
b.Sweep.A += step.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
Report(_contactSolver.Constraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
int childIndex)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
Vector2 d = p2 - p1;
float lower = 0.0f, upper = input.MaxFraction;
int index = -1;
for (int i = 0; i < Vertices.Count; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vector2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(false);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return(false);
}
}
Debug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Fraction = lower;
output.Normal = MathUtils.Multiply(ref transform.R, Normals[index]);
return(true);
}
return(false);
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
{
sc = Vector2.zero;
//Transform plane into shape co-ordinates
Vector2 normalL = MathUtils.MultiplyT(ref xf.R, normal);
float offsetL = offset - Vector2.Dot(normal, xf.Position);
float[] depths = new float[Settings.MaxPolygonVertices];
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for (i = 0; i < Vertices.Count; i++)
{
depths[i] = Vector2.Dot(normalL, Vertices[i]) - offsetL;
bool isSubmerged = depths[i] < -Settings.Epsilon;
if (i > 0)
{
if (isSubmerged)
{
if (!lastSubmerged)
{
intoIndex = i - 1;
diveCount++;
}
}
else
{
if (lastSubmerged)
{
outoIndex = i - 1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch (diveCount)
{
case 0:
if (lastSubmerged)
{
//Completely submerged
sc = MathUtils.Multiply(ref xf, MassData.Centroid);
return(MassData.Mass / Density);
}
else
{
//Completely dry
return(0);
}
#pragma warning disable 0162
break;
#pragma warning restore
case 1:
if (intoIndex == -1)
{
intoIndex = Vertices.Count - 1;
}
else
{
outoIndex = Vertices.Count - 1;
}
break;
}
int intoIndex2 = (intoIndex + 1) % Vertices.Count;
int outoIndex2 = (outoIndex + 1) % Vertices.Count;
float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
Vector2 intoVec = new Vector2(
Vertices[intoIndex].x * (1 - intoLambda) + Vertices[intoIndex2].x * intoLambda,
Vertices[intoIndex].y * (1 - intoLambda) + Vertices[intoIndex2].y * intoLambda);
Vector2 outoVec = new Vector2(
Vertices[outoIndex].x * (1 - outoLambda) + Vertices[outoIndex2].x * outoLambda,
Vertices[outoIndex].y * (1 - outoLambda) + Vertices[outoIndex2].y * outoLambda);
//Initialize accumulator
float area = 0;
Vector2 center = new Vector2(0, 0);
Vector2 p2 = Vertices[intoIndex2];
Vector2 p3;
float k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while (i != outoIndex2)
{
i = (i + 1) % Vertices.Count;
if (i == outoIndex2)
{
p3 = outoVec;
}
else
{
p3 = Vertices[i];
}
//Add the triangle formed by intoVec,p2,p3
{
Vector2 e1 = p2 - intoVec;
Vector2 e2 = p3 - intoVec;
float D = MathUtils.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
//
p2 = p3;
}
//Normalize and transform centroid
center *= 1.0f / area;
sc = MathUtils.Multiply(ref xf, center);
return(area);
}
/// <summary>
/// This makes the explosive explode
/// </summary>
/// <param name="pos">
/// The position where the explosion happens
/// </param>
/// <param name="radius">
/// The explosion radius
/// </param>
/// <param name="maxForce">
/// The explosion force at the explosion point
/// (then is inversely proportional to the square of the distance)
/// </param>
/// <returns>
/// A dictionnary containing all the "exploded" fixtures
/// with a list of the applied impulses
/// </returns>
public Dictionary <Fixture, List <Vector2> > Activate(Vector2 pos, float radius, float maxForce)
{
_exploded.Clear();
AABB aabb;
aabb.LowerBound = pos + new Vector2(-radius, -radius);
aabb.UpperBound = pos + new Vector2(radius, radius);
Fixture[] shapes = new Fixture[MaxShapes];
// More than 5 shapes in an explosion could be possible, but still strange.
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
// Query the world for overlapping shapes.
World.QueryAABB(
fixture =>
{
if (fixture.TestPoint(ref pos))
{
if (IgnoreWhenInsideShape)
{
exit = true;
}
else
{
containedShapes[containedShapeCount++] = fixture;
}
}
else
{
shapes[shapeCount++] = fixture;
}
// Continue the query.
return(true);
}, ref aabb);
if (exit)
{
return(_exploded);
}
// Per shape max/min angles for now.
float[] vals = new float[shapeCount * 2];
int valIndex = 0;
for (int i = 0; i < shapeCount; ++i)
{
PolygonShape ps;
CircleShape cs = shapes[i].Shape as CircleShape;
if (cs != null)
{
// We create a "diamond" approximation of the circle
Vertices v = new Vertices();
Vector2 vec = Vector2.zero + new Vector2(cs.Radius, 0);
v.Add(vec);
vec = Vector2.zero + new Vector2(0, cs.Radius);
v.Add(vec);
vec = Vector2.zero + new Vector2(-cs.Radius, cs.Radius);
v.Add(vec);
vec = Vector2.zero + new Vector2(0, -cs.Radius);
v.Add(vec);
ps = new PolygonShape(v, 0);
}
else
{
ps = shapes[i].Shape as PolygonShape;
}
if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
{
Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos;
float angleToCentroid = (float)Math.Atan2(toCentroid.y, toCentroid.x);
float min = float.MaxValue;
float max = float.MinValue;
float minAbsolute = 0.0f;
float maxAbsolute = 0.0f;
for (int j = 0; j < (ps.Vertices.Count()); ++j)
{
Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
float newAngle = (float)Math.Atan2(toVertex.y, toVertex.x);
float diff = (newAngle - angleToCentroid);
diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi);
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
if (diff < 0.0f)
{
diff += 2 * MathHelper.Pi; // correction for not handling negs
}
diff -= MathHelper.Pi;
if (Math.Abs(diff) > MathHelper.Pi)
{
throw new ArgumentException("OMG!");
}
// Something's wrong, point not in shape but exists angle diff > 180
if (diff > max)
{
max = diff;
maxAbsolute = newAngle;
}
if (diff < min)
{
min = diff;
minAbsolute = newAngle;
}
}
vals[valIndex] = minAbsolute;
++valIndex;
vals[valIndex] = maxAbsolute;
++valIndex;
}
}
Array.Sort(vals, 0, valIndex, _rdc);
_data.Clear();
bool rayMissed = true;
for (int i = 0; i < valIndex; ++i)
{
Fixture shape = null;
float midpt;
int iplus = (i == valIndex - 1 ? 0 : i + 1);
if (vals[i] == vals[iplus])
{
continue;
}
if (i == valIndex - 1)
{
// the single edgecase
midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]);
}
else
{
midpt = (vals[i + 1] + vals[i]);
}
midpt = midpt / 2;
Vector2 p1 = pos;
Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt),
(float)Math.Sin(midpt)) + pos;
// RaycastOne
bool hitClosest = false;
World.RayCast((f, p, n, fr) =>
{
Body body = f.Body;
if (!IsActiveOn(body))
{
return(0);
}
if (body.UserData != null)
{
int index = (int)body.UserData;
if (index == 0)
{
// filter
return(-1.0f);
}
}
hitClosest = true;
shape = f;
return(fr);
}, p1, p2);
//draws radius points
if ((hitClosest) && (shape.Body.BodyType == BodyType.Dynamic))
{
if ((_data.Count() > 0) && (_data.Last().Body == shape.Body) && (!rayMissed))
{
int laPos = _data.Count - 1;
ShapeData la = _data[laPos];
la.Max = vals[iplus];
_data[laPos] = la;
}
else
{
// make new
ShapeData d;
d.Body = shape.Body;
d.Min = vals[i];
d.Max = vals[iplus];
_data.Add(d);
}
if ((_data.Count() > 1) &&
(i == valIndex - 1) &&
(_data.Last().Body == _data.First().Body) &&
(_data.Last().Max == _data.First().Min))
{
ShapeData fi = _data[0];
fi.Min = _data.Last().Min;
_data.RemoveAt(_data.Count() - 1);
_data[0] = fi;
while (_data.First().Min >= _data.First().Max)
{
fi.Min -= MathHelper.Pi * 2;
_data[0] = fi;
}
}
int lastPos = _data.Count - 1;
ShapeData last = _data[lastPos];
while ((_data.Count() > 0) &&
(_data.Last().Min >= _data.Last().Max)) // just making sure min<max
{
last.Min = _data.Last().Min - 2 * MathHelper.Pi;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count(); ++i)
{
if (!IsActiveOn(_data[i].Body))
{
continue;
}
float arclen = _data[i].Max - _data[i].Min;
float first = MathHelper.Min(MaxEdgeOffset, EdgeRatio * arclen);
int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle);
if (insertedRays < 0)
{
insertedRays = 0;
}
float offset = (arclen - first * 2.0f) / ((float)MinRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on floats.
//Added FloatEquals with a large epsilon.
for (float j = _data[i].Min + first;
j < _data[i].Max || MathUtils.FloatEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
Vector2 p1 = pos;
Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
Vector2 hitpoint = Vector2.zero;
float minlambda = float.MaxValue;
List <Fixture> fl = _data[i].Body.FixtureList;
for (int x = 0; x < fl.Count; x++)
{
Fixture f = fl[x];
RayCastInput ri;
ri.Point1 = p1;
ri.Point2 = p2;
ri.MaxFraction = 50f;
RayCastOutput ro;
if (f.RayCast(out ro, ref ri, 0))
{
if (minlambda > ro.Fraction)
{
minlambda = ro.Fraction;
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
}
}
// the force that is to be applied for this particular ray.
// offset is angular coverage. lambda*length of segment is distance.
float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi *
(1.0f - Math.Min(1.0f, minlambda));
// We Apply the impulse!!!
Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j),
(float)Math.Sin(j)), -ro.Normal) *
new Vector2((float)Math.Cos(j),
(float)Math.Sin(j));
_data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
// We gather the fixtures for returning them
Vector2 val = Vector2.zero;
List <Vector2> vectorList;
if (_exploded.TryGetValue(f, out vectorList))
{
val.x += Math.Abs(vectImp.x);
val.y += Math.Abs(vectImp.y);
vectorList.Add(val);
}
else
{
vectorList = new List <Vector2>();
val.x = Math.Abs(vectImp.x);
val.y = Math.Abs(vectImp.y);
vectorList.Add(val);
_exploded.Add(f, vectorList);
}
if (minlambda > 1.0f)
{
hitpoint = p2;
}
}
}
}
// We check contained shapes
for (int i = 0; i < containedShapeCount; ++i)
{
Fixture fix = containedShapes[i];
if (!IsActiveOn(fix.Body))
{
continue;
}
float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi;
Vector2 hitPoint;
CircleShape circShape = fix.Shape as CircleShape;
if (circShape != null)
{
hitPoint = fix.Body.GetWorldPoint(circShape.Position);
}
else
{
PolygonShape shape = fix.Shape as PolygonShape;
hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid);
}
Vector2 vectImp = impulse * (hitPoint - pos);
List <Vector2> vectorList = new List <Vector2>();
vectorList.Add(vectImp);
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
if (!_exploded.ContainsKey(fix))
{
_exploded.Add(fix, vectorList);
}
}
return(_exploded);
}
public static void Set(ref SimplexCache cache,
DistanceProxy proxyA, ref Sweep sweepA,
DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
_axis = new Vector2(a.y, -a.x);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
_axis = new Vector2(a.y, -a.x);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
public static float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
float wA = c.BodyA.AngularVelocityInternal;
float wB = c.BodyB.AngularVelocityInternal;
float tangentx = c.Normal.y;
float tangenty = -c.Normal.x;
float friction = c.Friction;
Debug.Assert(c.PointCount == 1 || c.PointCount == 2);
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
float lambda = ccp.TangentMass *
-((c.BodyB.LinearVelocityInternal.x + (-wB * ccp.rB.y) -
c.BodyA.LinearVelocityInternal.x - (-wA * ccp.rA.y)) * tangentx +
(c.BodyB.LinearVelocityInternal.y + (wB * ccp.rB.x) -
c.BodyA.LinearVelocityInternal.y - (wA * ccp.rA.x)) * tangenty);
// MathUtils.Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction));
lambda = newImpulse - ccp.TangentImpulse;
// Apply contact impulse
float px = lambda * tangentx;
float py = lambda * tangenty;
c.BodyA.LinearVelocityInternal.x -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.y -= c.BodyA.InvMass * py;
wA -= c.BodyA.InvI * (ccp.rA.x * py - ccp.rA.y * px);
c.BodyB.LinearVelocityInternal.x += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.y += c.BodyB.InvMass * py;
wB += c.BodyB.InvI * (ccp.rB.x * py - ccp.rB.y * px);
ccp.TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
// Compute normal impulse
float lambda = -ccp.NormalMass *
((c.BodyB.LinearVelocityInternal.x + (-wB * ccp.rB.y) -
c.BodyA.LinearVelocityInternal.x - (-wA * ccp.rA.y)) * c.Normal.x +
(c.BodyB.LinearVelocityInternal.y + (wB * ccp.rB.x) -
c.BodyA.LinearVelocityInternal.y -
(wA * ccp.rA.x)) * c.Normal.y - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
float px = lambda * c.Normal.x;
float py = lambda * c.Normal.y;
c.BodyA.LinearVelocityInternal.x -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.y -= c.BodyA.InvMass * py;
wA -= c.BodyA.InvI * (ccp.rA.x * py - ccp.rA.y * px);
c.BodyB.LinearVelocityInternal.x += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.y += c.BodyB.InvMass * py;
wB += c.BodyB.InvI * (ccp.rB.x * py - ccp.rB.y * px);
ccp.NormalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
ContactConstraintPoint cp1 = c.Points[0];
ContactConstraintPoint cp2 = c.Points[1];
float ax = cp1.NormalImpulse;
float ay = cp2.NormalImpulse;
Debug.Assert(ax >= 0.0f && ay >= 0.0f);
// Relative velocity at contact
// Compute normal velocity
float vn1 = (c.BodyB.LinearVelocityInternal.x + (-wB * cp1.rB.y) - c.BodyA.LinearVelocityInternal.x -
(-wA * cp1.rA.y)) * c.Normal.x +
(c.BodyB.LinearVelocityInternal.y + (wB * cp1.rB.x) - c.BodyA.LinearVelocityInternal.y -
(wA * cp1.rA.x)) * c.Normal.y;
float vn2 = (c.BodyB.LinearVelocityInternal.x + (-wB * cp2.rB.y) - c.BodyA.LinearVelocityInternal.x -
(-wA * cp2.rA.y)) * c.Normal.x +
(c.BodyB.LinearVelocityInternal.y + (wB * cp2.rB.x) - c.BodyA.LinearVelocityInternal.y -
(wA * cp2.rA.x)) * c.Normal.y;
float bx = vn1 - cp1.VelocityBias - (c.K.Col1.x * ax + c.K.Col2.x * ay);
float by = vn2 - cp2.VelocityBias - (c.K.Col1.y * ax + c.K.Col2.y * ay);
float xx = -(c.NormalMass.Col1.x * bx + c.NormalMass.Col2.x * by);
float xy = -(c.NormalMass.Col1.y * bx + c.NormalMass.Col2.y * by);
while (true)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
if (xx >= 0.0f && xy >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.x;
float p1y = dx * c.Normal.y;
float p2x = dy * c.Normal.x;
float p2y = dy * c.Normal.y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.x -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.x * p1y - cp1.rA.y * p1x) + (cp2.rA.x * p2y - cp2.rA.y * p2x));
c.BodyB.LinearVelocityInternal.x += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.x * p1y - cp1.rB.y * p1x) + (cp2.rB.x * p2y - cp2.rB.y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
float k_errorTol = 1e-3f;
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
xx = -cp1.NormalMass * bx;
xy = 0.0f;
vn1 = 0.0f;
vn2 = c.K.Col1.y * xx + by;
if (xx >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.x;
float p1y = dx * c.Normal.y;
float p2x = dy * c.Normal.x;
float p2y = dy * c.Normal.y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.x -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.x * p1y - cp1.rA.y * p1x) + (cp2.rA.x * p2y - cp2.rA.y * p2x));
c.BodyB.LinearVelocityInternal.x += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.x * p1y - cp1.rB.y * p1x) + (cp2.rB.x * p2y - cp2.rB.y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
xx = 0.0f;
xy = -cp2.NormalMass * by;
vn1 = c.K.Col2.x * xy + bx;
vn2 = 0.0f;
if (xy >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.x;
float p1y = dx * c.Normal.y;
float p2x = dy * c.Normal.x;
float p2y = dy * c.Normal.y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.x -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.x * p1y - cp1.rA.y * p1x) + (cp2.rA.x * p2y - cp2.rA.y * p2x));
c.BodyB.LinearVelocityInternal.x += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.x * p1y - cp1.rB.y * p1x) + (cp2.rB.x * p2y - cp2.rB.y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
xx = 0.0f;
xy = 0.0f;
vn1 = bx;
vn2 = by;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.x;
float p1y = dx * c.Normal.y;
float p2x = dy * c.Normal.x;
float p2y = dy * c.Normal.y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.x -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.x * p1y - cp1.rA.y * p1x) + (cp2.rA.x * p2y - cp2.rA.y * p2x));
c.BodyB.LinearVelocityInternal.x += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.x * p1y - cp1.rB.y * p1x) + (cp2.rB.x * p2y - cp2.rB.y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
c.BodyA.AngularVelocityInternal = wA;
c.BodyB.AngularVelocityInternal = wB;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.x;
_impulse.Y = f2r.y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.x;
_impulse.Y += df.y;
Vector2 P = df.x * _perp;
float L1 = df.x * _s1 + df.y;
float L2 = df.x * _s2 + df.y;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
LocalCenterA = Vector2.zero;
LocalCenterB = bB.LocalCenter;
Transform xf2;
bB.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = LocalAnchorA;
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1;
InvMassA = 0.0f;
InvIA = 0.0f;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = _localXAxis1;
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = _localYAxis1;
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
MotorForce = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
MotorForce *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis;
float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
MotorForce = 0.0f;
}
}
/// <summary>
/// This resets the mass properties to the sum of the mass properties of the fixtures.
/// This normally does not need to be called unless you called SetMassData to override
/// the mass and you later want to reset the mass.
/// </summary>
public void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
InvMass = 0.0f;
_inertia = 0.0f;
InvI = 0.0f;
Sweep.LocalCenter = Vector2.zero;
// Kinematic bodies have zero mass.
if (BodyType == BodyType.Kinematic)
{
Sweep.C0 = Sweep.C = Xf.Position;
return;
}
Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static);
// Accumulate mass over all fixtures.
Vector2 center = Vector2.zero;
foreach (Fixture f in FixtureList)
{
if (f.Shape._density == 0)
{
continue;
}
MassData massData = f.Shape.MassData;
_mass += massData.Mass;
center += massData.Mass * massData.Centroid;
_inertia += massData.Inertia;
}
//Static bodies only have mass, they don't have other properties. A little hacky tho...
if (BodyType == BodyType.Static)
{
Sweep.C0 = Sweep.C = Xf.Position;
return;
}
// Compute center of mass.
if (_mass > 0.0f)
{
InvMass = 1.0f / _mass;
center *= InvMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
_mass = 1.0f;
InvMass = 1.0f;
}
if (_inertia > 0.0f && (Flags & BodyFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
_inertia -= _mass * Vector2.Dot(center, center);
Debug.Assert(_inertia > 0.0f);
InvI = 1.0f / _inertia;
}
else
{
_inertia = 0.0f;
InvI = 0.0f;
}
// Move center of mass.
Vector2 oldCenter = Sweep.C;
Sweep.LocalCenter = center;
Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter);
// Update center of mass velocity.
Vector2 a = Sweep.C - oldCenter;
LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.y, AngularVelocityInternal * a.x);
}