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