public override void CopyTo(MathUtils.Graphs.IEdge edge)
{
base.CopyTo(edge);
if (this.Tokens != null)
{
(edge as ColouredArc).Tokens = new MultiSet<Function>(this.Tokens);
}
}
public override void CopyTo(MathUtils.Graphs.IVertex vertex)
{
base.CopyTo(vertex);
var place = vertex as ColouredPlace;
place.ColorSetName = this.ColorSetName;
place.InitFunction = this.InitFunction;
place.collection = this.collection;
}
/// <summary>
/// Initializes a new instance of the MapAround.CoordinateSystem.Transformations.Affine.
/// </summary>
public Affine(MathUtils.Matrix matrix)
: this(matrix, false)
{
}
void GetRaySplitParams(Point3D raystart, Vector3D raydir, out Edge edge0, out double param0, out MathUtils.RayLineResult res0, out Edge edge1, out double param1, out MathUtils.RayLineResult res1)
{
edge0 = null;
edge1 = null;
param0 = 0;
param1 = 0;
res0 = MathUtils.RayLineResult.UNKOWN;
res1 = MathUtils.RayLineResult.UNKOWN;
//find first intersecting edge
int i = 0;
for (; i < Edges.Count; i++)
{
double closestdist, closestu;
MathUtils.RayLineResult res = Edges[i].ClosestPointRay(out closestdist, out closestu, out param0, raystart, raydir);
edge0 = Edges[i];
//valid intersection if we touch the line, the first vertex or fully overlap the edge
if (res == MathUtils.RayLineResult.INTERSECTING_LINE || res == MathUtils.RayLineResult.INTERSECTING_POS0 || res == MathUtils.RayLineResult.PARALLEL_OVERLAPPING)
{
res0 = res;
i++;
break;
}
}
//find second intersecting edge
for (; i < Edges.Count; i++)
{
double closestdist, closestu;
MathUtils.RayLineResult res = Edges[i].ClosestPointRay(out closestdist, out closestu, out param1, raystart, raydir);
edge1 = Edges[i];
//valid intersection if we touch the line, the first vertex or fully overlap the edge
if (res == MathUtils.RayLineResult.INTERSECTING_LINE || res == MathUtils.RayLineResult.INTERSECTING_POS0 || res == MathUtils.RayLineResult.PARALLEL_OVERLAPPING)
{
res1 = res;
i++;
break;
}
}
}
//Extracted from Box2D
/// <summary>
/// Returns the convex hull from the given vertices.
/// </summary>
/// <param name="vertices">The vertices.</param>
public static Vertices GetConvexHull(Vertices vertices)
{
if (vertices.Count <= 3)
{
return(vertices);
}
// Find the right most point on the hull
var i0 = 0;
var x0 = vertices[0].x;
for (var i = 1; i < vertices.Count; ++i)
{
var x = vertices[i].x;
if (x > x0 || x == x0 && vertices[i].y < vertices[i0].y)
{
i0 = i;
x0 = x;
}
}
var hull = new int[vertices.Count];
var m = 0;
var ih = i0;
for (;;)
{
hull[m] = ih;
var ie = 0;
for (var j = 1; j < vertices.Count; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
var r = vertices[ie] - vertices[hull[m]];
var v = vertices[j] - vertices[hull[m]];
var c = MathUtils.Cross(ref r, ref v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.sqrMagnitude > r.sqrMagnitude)
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
var result = new Vertices(m);
// Copy vertices.
for (var i = 0; i < m; ++i)
{
result.Add(vertices[hull[i]]);
}
return(result);
}
private void WriteTokenInternal(BsonToken t)
{
switch (t.Type)
{
case BsonType.Object:
{
BsonObject value = (BsonObject)t;
_writer.Write(value.CalculatedSize);
foreach (BsonProperty property in value)
{
_writer.Write((sbyte)property.Value.Type);
WriteString((string)property.Name.Value, property.Name.ByteCount, null);
WriteTokenInternal(property.Value);
}
_writer.Write((byte)0);
}
break;
case BsonType.Array:
{
BsonArray value = (BsonArray)t;
_writer.Write(value.CalculatedSize);
ulong index = 0;
foreach (BsonToken c in value)
{
_writer.Write((sbyte)c.Type);
WriteString(index.ToString(CultureInfo.InvariantCulture), MathUtils.IntLength(index), null);
WriteTokenInternal(c);
index++;
}
_writer.Write((byte)0);
}
break;
case BsonType.Integer:
{
BsonValue value = (BsonValue)t;
_writer.Write(Convert.ToInt32(value.Value, CultureInfo.InvariantCulture));
}
break;
case BsonType.Long:
{
BsonValue value = (BsonValue)t;
_writer.Write(Convert.ToInt64(value.Value, CultureInfo.InvariantCulture));
}
break;
case BsonType.Number:
{
BsonValue value = (BsonValue)t;
_writer.Write(Convert.ToDouble(value.Value, CultureInfo.InvariantCulture));
}
break;
case BsonType.String:
{
BsonString value = (BsonString)t;
WriteString((string)value.Value, value.ByteCount, value.CalculatedSize - 4);
}
break;
case BsonType.Boolean:
_writer.Write(t == BsonBoolean.True);
break;
case BsonType.Null:
case BsonType.Undefined:
break;
case BsonType.Date:
{
BsonValue value = (BsonValue)t;
long ticks = 0;
if (value.Value is DateTime)
{
DateTime dateTime = (DateTime)value.Value;
if (DateTimeKindHandling == DateTimeKind.Utc)
{
dateTime = dateTime.ToUniversalTime();
}
else if (DateTimeKindHandling == DateTimeKind.Local)
{
dateTime = dateTime.ToLocalTime();
}
ticks = DateTimeUtils.ConvertDateTimeToJavaScriptTicks(dateTime, false);
}
#if HAVE_DATE_TIME_OFFSET
else
{
DateTimeOffset dateTimeOffset = (DateTimeOffset)value.Value;
ticks = DateTimeUtils.ConvertDateTimeToJavaScriptTicks(dateTimeOffset.UtcDateTime, dateTimeOffset.Offset);
}
#endif
_writer.Write(ticks);
}
break;
case BsonType.Binary:
{
BsonBinary value = (BsonBinary)t;
byte[] data = (byte[])value.Value;
_writer.Write(data.Length);
_writer.Write((byte)value.BinaryType);
_writer.Write(data);
}
break;
case BsonType.Oid:
{
BsonValue value = (BsonValue)t;
byte[] data = (byte[])value.Value;
_writer.Write(data);
}
break;
case BsonType.Regex:
{
BsonRegex value = (BsonRegex)t;
WriteString((string)value.Pattern.Value, value.Pattern.ByteCount, null);
WriteString((string)value.Options.Value, value.Options.ByteCount, null);
}
break;
default:
throw new ArgumentOutOfRangeException(nameof(t), "Unexpected token when writing BSON: {0}".FormatWith(CultureInfo.InvariantCulture, t.Type));
}
}
private int CalculateSize(BsonToken t)
{
switch (t.Type)
{
case BsonType.Object:
{
BsonObject value = (BsonObject)t;
int bases = 4;
foreach (BsonProperty p in value)
{
int size = 1;
size += CalculateSize(p.Name);
size += CalculateSize(p.Value);
bases += size;
}
bases += 1;
value.CalculatedSize = bases;
return(bases);
}
case BsonType.Array:
{
BsonArray value = (BsonArray)t;
int size = 4;
ulong index = 0;
foreach (BsonToken c in value)
{
size += 1;
size += CalculateSize(MathUtils.IntLength(index));
size += CalculateSize(c);
index++;
}
size += 1;
value.CalculatedSize = size;
return(value.CalculatedSize);
}
case BsonType.Integer:
return(4);
case BsonType.Long:
return(8);
case BsonType.Number:
return(8);
case BsonType.String:
{
BsonString value = (BsonString)t;
string s = (string)value.Value;
value.ByteCount = (s != null) ? Encoding.GetByteCount(s) : 0;
value.CalculatedSize = CalculateSizeWithLength(value.ByteCount, value.IncludeLength);
return(value.CalculatedSize);
}
case BsonType.Boolean:
return(1);
case BsonType.Null:
case BsonType.Undefined:
return(0);
case BsonType.Date:
return(8);
case BsonType.Binary:
{
BsonBinary value = (BsonBinary)t;
byte[] data = (byte[])value.Value;
value.CalculatedSize = 4 + 1 + data.Length;
return(value.CalculatedSize);
}
case BsonType.Oid:
return(12);
case BsonType.Regex:
{
BsonRegex value = (BsonRegex)t;
int size = 0;
size += CalculateSize(value.Pattern);
size += CalculateSize(value.Options);
value.CalculatedSize = size;
return(value.CalculatedSize);
}
default:
throw new ArgumentOutOfRangeException(nameof(t), "Unexpected token when writing BSON: {0}".FormatWith(CultureInfo.InvariantCulture, t.Type));
}
}
/** 判定几率 */
public bool randomProb(int prob, int max)
{
return(MathUtils.randomProb(prob, max));
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
}
// Compute the effective mass matrix.
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.X * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1.LinearVelocityInternal -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
b2.LinearVelocityInternal += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
public static float PtLineDist(float x1, float y1, float x2, float y2,
float px, float py)
{
return(MathUtils.Sqrt(PtLineDistSq(x1, y1, x2, y2, px, py)));
}
public float Distance(Vector2f point)
{
return(MathUtils.Sqrt(DistanceSquared(point)));
}
/// <summary>
/// Call this to draw shapes and other debug draw data.
/// </summary>
private void DrawDebugData()
{
if ((Flags & DebugViewFlags.Shape) == DebugViewFlags.Shape)
{
foreach (Body b in World.BodyList)
{
Transform xf;
b.GetTransform(out xf);
foreach (Fixture f in b.FixtureList)
{
if (b.Enabled == false)
{
DrawShape(f, xf, InactiveShapeColor);
}
else if (b.BodyType == BodyType.Static)
{
DrawShape(f, xf, StaticShapeColor);
}
else if (b.BodyType == BodyType.Kinematic)
{
DrawShape(f, xf, KinematicShapeColor);
}
else if (b.Awake == false)
{
DrawShape(f, xf, SleepingShapeColor);
}
else
{
DrawShape(f, xf, DefaultShapeColor);
}
}
}
}
if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints)
{
const float axisScale = 0.3f;
for (int i = 0; i < _pointCount; ++i)
{
ContactPoint point = _points[i];
if (point.State == PointState.Add)
{
DrawPoint(point.Position, 0.1f, new Color(0.3f, 0.95f, 0.3f));
}
else if (point.State == PointState.Persist)
{
DrawPoint(point.Position, 0.1f, new Color(0.3f, 0.3f, 0.95f));
}
if ((Flags & DebugViewFlags.ContactNormals) == DebugViewFlags.ContactNormals)
{
Vector2 p1 = point.Position;
Vector2 p2 = p1 + axisScale * point.Normal;
DrawSegment(p1, p2, new Color(0.4f, 0.9f, 0.4f));
}
}
_pointCount = 0;
}
if ((Flags & DebugViewFlags.PolygonPoints) == DebugViewFlags.PolygonPoints)
{
foreach (Body body in World.BodyList)
{
foreach (Fixture f in body.FixtureList)
{
PolygonShape polygon = f.Shape as PolygonShape;
if (polygon != null)
{
Transform xf;
body.GetTransform(out xf);
for (int i = 0; i < polygon.Vertices.Count; i++)
{
Vector2 tmp = MathUtils.Mul(ref xf, polygon.Vertices[i]);
DrawPoint(tmp, 0.1f, Color.Red);
}
}
}
}
}
if ((Flags & DebugViewFlags.Joint) == DebugViewFlags.Joint)
{
foreach (Joint j in World.JointList)
{
DrawJoint(j);
}
}
if ((Flags & DebugViewFlags.AABB) == DebugViewFlags.AABB)
{
Color color = new Color(0.9f, 0.3f, 0.9f);
IBroadPhase bp = World.ContactManager.BroadPhase;
foreach (Body body in World.BodyList)
{
if (body.Enabled == false)
{
continue;
}
foreach (Fixture f in body.FixtureList)
{
for (int t = 0; t < f.ProxyCount; ++t)
{
FixtureProxy proxy = f.Proxies[t];
AABB aabb;
bp.GetFatAABB(proxy.ProxyId, out aabb);
DrawAABB(ref aabb, color);
}
}
}
}
if ((Flags & DebugViewFlags.CenterOfMass) == DebugViewFlags.CenterOfMass)
{
foreach (Body b in World.BodyList)
{
Transform xf;
b.GetTransform(out xf);
xf.p = b.WorldCenter;
DrawTransform(ref xf);
}
}
if ((Flags & DebugViewFlags.Controllers) == DebugViewFlags.Controllers)
{
for (int i = 0; i < World.ControllerList.Count; i++)
{
Controller controller = World.ControllerList[i];
BuoyancyController buoyancy = controller as BuoyancyController;
if (buoyancy != null)
{
AABB container = buoyancy.Container;
DrawAABB(ref container, Color.LightBlue);
}
}
}
if ((Flags & DebugViewFlags.DebugPanel) == DebugViewFlags.DebugPanel)
{
DrawDebugPanel();
}
}
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++GJKCalls;
}
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3 <int> saveA = new FixedArray3 <int>();
FixedArray3 <int> saveB = new FixedArray3 <int>();
//float distanceSqr1 = Settings.MaxFloat;
// Main iteration loop.
int iter = 0;
while (iter < Settings.MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex.V[i].IndexA;
saveB[i] = simplex.V[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d));
vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++GJKIters;
}
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
GJKMaxIters = Math.Max(GJKMaxIters, iter);
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length();
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = Vector2.Normalize(output.PointB - output.PointA);
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
internal void Solve3()
{
Vector2 w1 = V[0].W;
Vector2 w2 = V[1].W;
Vector2 w3 = V[2].W;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
Vector2 e12 = w2 - w1;
float w1e12 = Vector2.Dot(w1, e12);
float w2e12 = Vector2.Dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
Vector2 e13 = w3 - w1;
float w1e13 = Vector2.Dot(w1, e13);
float w3e13 = Vector2.Dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
Vector2 e23 = w3 - w2;
float w2e23 = Vector2.Dot(w2, e23);
float w3e23 = Vector2.Dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = MathUtils.Cross(e12, e13);
float d123_1 = n123 * MathUtils.Cross(w2, w3);
float d123_2 = n123 * MathUtils.Cross(w3, w1);
float d123_3 = n123 * MathUtils.Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
SimplexVertex v0_1 = V[0];
v0_1.A = 1.0f;
V[0] = v0_1;
Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float inv_d13 = 1.0f / (d13_1 + d13_2);
SimplexVertex v0_3 = V[0];
SimplexVertex v2_3 = V[2];
v0_3.A = d13_1 * inv_d13;
v2_3.A = d13_2 * inv_d13;
V[0] = v0_3;
V[2] = v2_3;
Count = 2;
V[1] = V[2];
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
SimplexVertex v1_4 = V[1];
v1_4.A = 1.0f;
V[1] = v1_4;
Count = 1;
V[0] = V[1];
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
SimplexVertex v2_5 = V[2];
v2_5.A = 1.0f;
V[2] = v2_5;
Count = 1;
V[0] = V[2];
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float inv_d23 = 1.0f / (d23_1 + d23_2);
SimplexVertex v1_6 = V[1];
SimplexVertex v2_6 = V[2];
v1_6.A = d23_1 * inv_d23;
v2_6.A = d23_2 * inv_d23;
V[1] = v1_6;
V[2] = v2_6;
Count = 2;
V[0] = V[2];
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
SimplexVertex v0_7 = V[0];
SimplexVertex v1_7 = V[1];
SimplexVertex v2_7 = V[2];
v0_7.A = d123_1 * inv_d123;
v1_7.A = d123_2 * inv_d123;
v2_7.A = d123_3 * inv_d123;
V[0] = v0_7;
V[1] = v1_7;
V[2] = v2_7;
Count = 3;
}
/// <summary>
/// Initializes a new instance of the MapAround.CoordinateSystem.Transformations.Affine.
/// </summary>
private Affine(MathUtils.Matrix matrix, bool isInverse)
{
if (matrix == null)
throw new ArgumentNullException("matrix");
if (matrix.Size != 3)
throw new ArgumentException("Affine transform matrix size should be equal three.", "matrix");
if (matrix[0, 2] != 0 || matrix[1, 2] != 0 || matrix[2, 2] != 1)
throw new ArgumentException("Matrix does not define an affine transformation.", "matrix");
_matrix = matrix;
_isInverse = isInverse;
}
/// <summary>
/// Initializes a new instance of the MapAround.CoordinateSystem.Transformations.Affine.
/// </summary>
private Affine(MathUtils.Matrix matrix, MathUtils.Matrix inverseMatrix, bool isInverse)
{
if (inverseMatrix == null)
throw new ArgumentNullException("inverseMatrix");
_matrix = matrix;
_inverseMatrix = inverseMatrix;
_isInverse = isInverse;
}
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;
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
w1 -= i1 * ( /* r1 x P */_tmpFloat1 + impulse.Z);
v2 += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
w2 += i2 * ( /* r2 x P */_tmpFloat1 + impulse.Z);
}
else
{
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
_tmpVector1 = LocalAnchorA - b1.LocalCenter;
_tmpVector2 = LocalAnchorB - b2.LocalCenter;
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
w1 -= i1 * /* r1 x impulse */ _tmpFloat1;
v2 += m2 * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
w2 += i2 * /* r2 x impulse */ _tmpFloat1;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
Body b2 = BodyB;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b2.Sweep.A += b2.InvI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
positionError = C.Length();
float invMass1 = b1.InvMass, invMass2 = b2.InvMass;
float invI1 = b1.InvI, invI2 = b2.InvI;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
b2.Sweep.C += k_beta * invMass2 * impulse2;
C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1;
b2.Sweep.C += b2.InvMass * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
/** 随机一个整形 */
public int randomInt(int range)
{
return(MathUtils.randomInt(range));
}
