public static Mat22 Abs(Mat22 A) { Mat22 B = new Mat22(); B.Set(MathB2.Abs(A.Col1), MathB2.Abs(A.Col2)); return(B); }
public static Vec2 Abs(Vec2 a) { Vec2 b = new Vec2(); b.Set(MathB2.Abs(a.X), MathB2.Abs(a.Y)); return(b); }
public static Vec2 Max(Vec2 a, Vec2 b) { Vec2 c = new Vec2 { X = MathB2.Max(a.X, b.X), Y = MathB2.Max(a.Y, b.Y) }; return(c); }
public override void Step(Settings settings) { base.Step(settings); DistanceInput input = new DistanceInput(); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; SimplexCache cache = new SimplexCache(); cache.Count = 0; Collision.Distance(out DistanceOutput output, ref cache, ref input, _polygonA, _polygonB); StringBuilder strBld = new StringBuilder(); strBld.AppendFormat("distance = {0}", new object[] { output.Distance }); OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString()); _textLine += 15; strBld = new StringBuilder(); strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations }); OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString()); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); int i; for (i = 0; i < _polygonA.VertexCount; ++i) { _dv[i] = MathB2.Mul(_transformA, _polygonA.Vertices[i]); } _debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color); for (i = 0; i < _polygonB.VertexCount; ++i) { _dv[i] = MathB2.Mul(_transformB, _polygonB.Vertices[i]); } _debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color); } Vec2 x1 = output.PointA; Vec2 x2 = output.PointB; OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0)); OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0)); OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0)); }
public override void Step(TimeStep step) { //B2_NOT_USED(step); if (InvSqr) { for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) { Body body1 = i.body; for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) { Body body2 = j.body; Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); float r2 = d.LengthSquared(); if (r2 < Settings.FLT_EPSILON) { continue; } Vec2 f = G / r2 / MathB2.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d; body1.ApplyForce(f, body1.GetWorldCenter()); body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); } } } else { for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) { Body body1 = i.body; for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) { Body body2 = j.body; Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); float r2 = d.LengthSquared(); if (r2 < Settings.FLT_EPSILON) { continue; } Vec2 f = G / r2 * body1.GetMass() * body2.GetMass() * d; body1.ApplyForce(f, body1.GetWorldCenter()); body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); } } } }
/// Calculate the angle that the rotation matrix represents. public float GetAngle() { return(MathB2.Atan2(R.Col1.Y, R.Col1.X)); }