public RopeTest()
{
const int N = 40;
Vec2[] vertices = new Vec2[N];
float [] masses = new float [N];
for (int i = 0; i < N; ++i)
{
vertices[i].Set(0.0f, 20.0f - 0.25f * i);
masses[i] = 1.0f;
}
masses[0] = 0.0f;
masses[1] = 0.0f;
RopeDef def = new RopeDef();
def.vertices = new List<Vec2>(vertices);
def.count = N;
def.gravity.Set(0.0f, -10.0f);
def.masses = new List<float>(masses);
def.damping = 0.1f;
def.k2 = 1.0f;
def.k3 = 0.5f;
m_rope.Initialize(def);
m_angle = 0.0f;
m_rope.SetAngle(m_angle);
}
public Test(){
Vec2 gravity = new Vec2();
gravity.Set(0.0f, -10.0f);
m_world = new World(gravity);
m_bomb = null;
m_textLine = 30;
m_mouseJoint = null;
m_pointCount = 0;
m_debugDraw = new DebugDraw();
m_destructionListener = new TestDestructionListener();
m_destructionListener.test = this;
m_world.SetDestructionListener(m_destructionListener);
m_world.SetContactListener(this);
m_world.SetDebugDraw(m_debugDraw);
m_bombSpawning = false;
m_stepCount = 0;
BodyDef bodyDef = new BodyDef();
m_groundBody = m_world.CreateBody(bodyDef);
m_maxProfile = new Profile();
m_totalProfile = new Profile();
}
/// Initialize the bodies, anchors, and reference angle using a world
/// anchor point.
// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// C = angle2 - angle1 - referenceAngle
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
public void Initialize(Body bA, Body bB, Vec2 anchor) {
bodyA = bA;
bodyB = bB;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
}
public override void DrawSolidCircle(Vec2 center, float radius, Vec2 axis, Color color) {
float k_segments = 16.0f;
float k_increment = 2.0f * (float)System.Math.PI / k_segments;
float theta = 0.0f;
GL.Color3(0.5f * color.R, 0.5f * color.G, 0.5f * color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.TriangleFan);
for (int i = 0; i < k_segments; ++i) {
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex2(v.X, v.Y);
theta += k_increment;
}
GL.End();
theta = 0.0f;
GL.Color4(color.R, color.G, color.B, 1.0f);
GL.Begin(BeginMode.LineLoop);
for (int i = 0; i < k_segments; ++i) {
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex2(v.X, v.Y);
theta += k_increment;
}
GL.End();
Vec2 p = center + radius * axis;
GL.Begin(BeginMode.Lines);
GL.Vertex2(center.X, center.Y);
GL.Vertex2(p.X, p.Y);
GL.End();
GL.Enable(EnableCap.Texture2D);
}
/// Use this to update the target point.
public void SetTarget(Vec2 target){
if (m_bodyB.IsAwake() == false)
{
m_bodyB.SetAwake(true);
}
m_targetA = target;
}
/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
// Linear constraint (point-to-line)
// d = pB - pA = xB + rB - xA - rA
// C = dot(ay, d)
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
// Spring linear constraint
// C = dot(ax, d)
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
// Motor rotational constraint
// Cdot = wB - wA
// J = [0 0 -1 0 0 1]
public void Initialize(Body bA, Body bB, Vec2 anchor, Vec2 axis) {
bodyA = bA;
bodyB = bB;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
localAxisA = bodyA.GetLocalVector(axis);
}
public override void Step(TestSettings settings)
{
Manifold manifold;
Collision.CollidePolygons(out manifold, m_polygonA, m_transformA, m_polygonB, m_transformB);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, m_transformA, m_polygonA.m_radius, m_transformB, m_polygonB.m_radius);
m_debugDraw.DrawString("point count = {0}", manifold.points.Count());
{
Color color = Color.FromArgb(225, 225, 225);
Vec2[] v = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < m_polygonA.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);
for (int i = 0; i < m_polygonB.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
}
for (int i = 0; i < manifold.points.Count(); ++i)
{
m_debugDraw.DrawPoint(worldManifold.points[i], 4.0f, Color.FromArgb(225, 75, 75));
}
}
/// Set/get the target linear offset, in frame A, in meters.
public void SetLinearOffset(Vec2 linearOffset){
if (linearOffset.X != m_linearOffset.X || linearOffset.Y != m_linearOffset.Y)
{
m_bodyA.SetAwake(true);
m_bodyB.SetAwake(true);
m_linearOffset = linearOffset;
}
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
// 1-D constrained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public void Initialize(Body b1, Body b2,
Vec2 anchor1, Vec2 anchor2) {
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
Vec2 d = anchor2 - anchor1;
length = d.Length();
}
public class RopeDef { //was struct
public RopeDef() {
vertices = new List<Vec2>();
count = 0;
masses = new List<float>();
gravity = new Vec2(0, 0);
damping = 0.1f;
k2 = 0.9f;
k3 = 0.1f;
}
public override float ReportFixture(Fixture fixture, Vec2 point,
Vec2 normal, float fraction)
{
m_fixture = fixture;
m_point = point;
m_normal = normal;
return fraction;
}
public override void DrawPolygon(Vec2[] vertices, int vertexCount, Color color) {
GL.Color3(color.R, color.G, color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.LineLoop);
for (int i = 0; i < vertexCount; ++i) {
GL.Vertex2(vertices[i].X, vertices[i].Y);
}
GL.End();
GL.Enable(EnableCap.Texture2D);
}
public override void DrawSegment(Vec2 p1, Vec2 p2, Color color) {
GL.Color3(color.R, color.G, color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.Lines);
{
GL.Vertex2(p1.X, p1.Y);
GL.Vertex2(p2.X, p2.Y);
}
GL.End();
GL.Enable(EnableCap.Texture2D);
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases. Solve only the upper
/// 2-by-2 matrix equation.
public Vec2 Solve22(Vec2 b) {
float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y;
float det = a11 * a22 - a12 * a21;
if (det != 0.0f) {
det = 1.0f / det;
}
Vec2 x;
x.X = det * (a22 * b.X - a12 * b.Y);
x.Y = det * (a11 * b.Y - a21 * b.X);
return x;
}
/// Get the supporting vertex index in the given direction.
public int GetSupport(Vec2 d){
int bestIndex = 0;
float bestValue = Utilities.Dot(m_vertices[0], d);
for (int i = 1; i < m_vertices.Count(); ++i) {
float value = Utilities.Dot(m_vertices[i], d);
if (value > bestValue) {
bestIndex = i;
bestValue = value;
}
}
return bestIndex;
}
public GravityTest() {
// Define the gravity vector.
Vec2 gravity = new Vec2(0.0f, 10.0f);
// Construct a world object, which will hold and simulate the rigid bodies.
m_world.SetGravity(gravity);
// Define the ground body.
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0.0f, 20.0f);
// Call the body factory which allocates memory for the ground body
// from a pool and creates the ground box shape (also from a pool).
// The body is also added to the world.
Body groundBody = m_world.CreateBody(groundBodyDef);
// Define the ground box shape.
PolygonShape groundBox = new PolygonShape();
// The extents are the half-widths of the box.
groundBox.SetAsBox(50.0f, 10.0f);
groundBox.Density = 0;
// Add the ground fixture to the ground body.
groundBody.CreateFixture(groundBox);
// Define the dynamic body. We set its position and call the body factory.
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType._dynamicBody;
bodyDef.Position = new Vec2(0.0f, 4.0f);
Body body = m_world.CreateBody(bodyDef);
// Define another box shape for our dynamic body.
PolygonShape dynamicBox = new PolygonShape();
dynamicBox.SetAsBox(1.0f, 1.0f);
// Define the dynamic body fixture.
FixtureDef fixtureDef = new FixtureDef();
fixtureDef.shape = dynamicBox;
// Set the box Density to be non-zero, so it will be dynamic.
fixtureDef.Density = 1.0f;
// Override the default friction.
fixtureDef.friction = 0.3f;
// Add the shape to the body.
body.CreateFixture(fixtureDef);
}
public override void DrawCircle(Vec2 center, float radius, Color color) {
float k_segments = 16.0f;
float k_increment = 2.0f * (float)System.Math.PI / k_segments;
float theta = 0.0f;
GL.Color3(color.R, color.G, color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.LineLoop);
for (int i = 0; i < k_segments; ++i) {
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex2(v.X, v.Y);
theta += k_increment;
}
GL.End();
GL.Enable(EnableCap.Texture2D);
}
/// This constructor sets the body definition default values.
public BodyDef() {
UserData = null;
Position = new Vec2(0.0f, 0.0f);
angle = 0.0f;
linearVelocity = new Vec2(0.0f, 0.0f);
angularVelocity = 0.0f;
linearDamping = 0.0f;
angularDamping = 0.0f;
allowSleep = true;
awake = true;
fixedRotation = false;
bullet = false;
type = BodyType._staticBody;
active = true;
gravityScale = 1.0f;
}
public void Step(TestSettings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.proxyA.Set(m_polygonA, 0);
input.proxyB.Set(m_polygonB, 0);
input.transformA = m_transformA;
input.transformB = m_transformB;
input.useRadii = true;
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceOutput output;
Utilities.Distance(out output, cache, input);
m_debugDraw.DrawString("distance = %g", output.distance);
m_debugDraw.DrawString("iterations = %d", output.iterations);
{
Color color = Color.FromArgb(225, 225, 225);
Vec2[] v = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < m_polygonA.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);
for (int i = 0; i < m_polygonB.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
}
Vec2 x1 = output.pointA;
Vec2 x2 = output.pointB;
Color c1 = Color.FromArgb(255, 0, 0);
m_debugDraw.DrawPoint(x1, 4.0f, c1);
Color c2 = Color.FromArgb(255, 255, 0);
m_debugDraw.DrawPoint(x2, 4.0f, c2);
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
// Pulley:
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
// C = C0 - (length1 + ratio * length2)
// u1 = (p1 - s1) / norm(p1 - s1)
// u2 = (p2 - s2) / norm(p2 - s2)
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
public void Initialize(Body bA, Body bB,
Vec2 groundA, Vec2 groundB,
Vec2 anchorA, Vec2 anchorB,
float r) {
bodyA = bA;
bodyB = bB;
groundAnchorA = groundA;
groundAnchorB = groundB;
localAnchorA = bodyA.GetLocalPoint(anchorA);
localAnchorB = bodyB.GetLocalPoint(anchorB);
Vec2 dA = anchorA - groundA;
lengthA = dA.Length();
Vec2 dB = anchorB - groundB;
lengthB = dB.Length();
ratio = r;
Utilities.Assert(ratio > Single.Epsilon);
}
public void Generate()
{
Vec2 lowerBound = new Vec2(-8.0f, -8.0f);
Vec2 upperBound = new Vec2(8.0f, 8.0f);
for (int i = 0; i < e_count; ++i)
{
float x = 10.0f * RandomFloat();
float y = 10.0f * RandomFloat();
// Clamp onto a square to help create collinearities.
// This will stress the convex hull algorithm.
Vec2 v = new Vec2(x, y);
v = Utilities.Clamp(v, lowerBound, upperBound);
m_points[i] = v;
}
}
public Prismatic() {
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
shape.Density = 5;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-10.0f, 10.0f);
bd.angle = 0.5f * (float)Math.PI;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
PrismaticJointDef pjd = new PrismaticJointDef();
// Bouncy limit
Vec2 axis = new Vec2(2.0f, 1.0f);
axis.Normalize();
pjd.Initialize(ground, body, new Vec2(0.0f, 0.0f), axis);
// Non-bouncy limit
//pjd.Initialize(ground, body, new Vec2(-10.0f, 10.0f), new Vec2(1.0f, 0.0f));
pjd.motorSpeed = 10.0f;
pjd.maxMotorForce = 10000.0f;
pjd.enableMotor = true;
pjd.lowerTranslation = 0.0f;
pjd.upperTranslation = 20.0f;
pjd.enableLimit = true;
m_joint = (PrismaticJoint)m_world.CreateJoint(pjd);
}
}
public Chain()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.6f, 0.125f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 20.0f;
fd.friction = 0.2f;
RevoluteJointDef jd = new RevoluteJointDef();
jd.collideConnected = false;
const float y = 25.0f;
Body prevBody = ground;
for (int i = 0; i < 30; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.5f + i, y);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
Vec2 anchor = new Vec2((float)(i), y);
jd.Initialize(prevBody, body, anchor);
m_world.CreateJoint(jd);
prevBody = body;
}
}
}
/// Create a chain with isolated end vertices.
/// @param vertices an array of vertices, these are copied
/// @param count the vertex count
public void CreateChain(Vec2[] vertices, int count){
throw new NotImplementedException();
//Utilities.Assert(m_vertices == null && m_count == 0);
//Utilities.Assert(count >= 2);
//for (int i = 1; i < count; ++i)
//{
// Vec2 v1 = vertices[i-1];
// Vec2 v2 = vertices[i];
// // If the code crashes here, it means your vertices are too close together.
// Utilities.Assert(DistanceSquared(v1, v2) >Settings._linearSlop *Settings._linearSlop);
//}
//m_count = count;
//m_vertices = (Vec2*)Alloc(count * sizeof(Vec2));
//memcpy(m_vertices, vertices, m_count * sizeof(Vec2));
//m_hasPrevVertex = false;
//m_hasNextVertex = false;
}
public Body AddNode(Body parent, Vec2 localAnchor, int depth, float offset, float a)
{
Vec2 h = new Vec2(0.0f, a);
Vec2 p = parent.GetPosition() + localAnchor - h;
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType._dynamicBody;
bodyDef.Position = p;
Body body = m_world.CreateBody(bodyDef);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f * a, a);
shape.Density = 20;
body.CreateFixture(shape);
if (depth == e_depth)
{
return body;
}
shape.SetAsBox(offset, 0.25f * a, new Vec2(0, -a), 0.0f);
body.CreateFixture(shape);
Vec2 a1 = new Vec2(offset, -a);
Vec2 a2 = new Vec2(-offset, -a);
Body body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a);
Body body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a);
RevoluteJointDef jointDef = new RevoluteJointDef();
jointDef.bodyA = body;
jointDef.localAnchorB = h;
jointDef.localAnchorA = a1;
jointDef.bodyB = body1;
m_world.CreateJoint(jointDef);
jointDef.localAnchorA = a2;
jointDef.bodyB = body2;
m_world.CreateJoint(jointDef);
return body;
}
public Pyramid()
{
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
float a = 0.5f;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, a);
shape.Density = 5;
Vec2 x = new Vec2(-7.0f, 0.75f);
Vec2 y;
Vec2 deltaX = new Vec2(0.5625f, 1.25f);
Vec2 deltaY = new Vec2(1.125f, 0.0f);
for (int i = 0; i < e_count; ++i)
{
y = x;
for (int j = i; j < e_count; ++j)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position = y;
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
y += deltaY;
}
x += deltaX;
}
}
}
void CreateCircle()
{
float radius = 2.0f;
CircleShape shape = new CircleShape();
shape.m_p.SetZero();
shape.m_radius = radius;
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
fd.friction = 0.0f;
Vec2 p = new Vec2(RandomFloat(), 3.0f + RandomFloat());
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position = p;
//bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
}
public TestSettings() {
viewCenter = new Vec2(0.0f, 20.0f);
hz = 60.0f;
velocityIterations = 8;
positionIterations = 3;
drawShapes = true;
drawJoints = true;
drawAABBs = false;
drawContactPoints = false;
drawContactNormals = false;
drawContactImpulse = false;
drawFrictionImpulse = false;
drawCOMs = true;
drawStats = false;
drawProfile = false;
enableWarmStarting = true;
enableContinuous = true;
enableSubStepping = false;
enableSleep = true;
pause = false;
singleStep = false;
}
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction)
{
Body body = fixture.GetBody();
if (body.UserData != null)
{
int index = (int)body.UserData;
if (index == 0)
{
// By returning -1, we instruct the calling code to ignore this fixture
// and continue the ray-cast to the next fixture.
return -1.0f;
}
}
m_hit = true;
m_point = point;
m_normal = normal;
// At this point we have a hit, so we know the ray is obstructed.
// By returning 0, we instruct the calling code to terminate the ray-cast.
return 0.0f;
}
public void DrawFixture(Fixture fixture)
{
Color color = Color.FromArgb(245, 245, 150);
Transform xf = fixture.GetBody().GetTransform();
switch (fixture.GetShapeType())
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.GetShape();
Vec2 center = Utilities.Mul(xf, circle.m_p);
float radius = circle.m_radius;
m_debugDraw.DrawCircle(center, radius, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.GetShape();
int vertexCount = poly.m_count;
Utilities.Assert(vertexCount <= Settings._maxPolygonVertices);
Vec2[] vertices = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = Utilities.Mul(xf, poly.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, vertexCount, color);
}
break;
default:
break;
}
}
internal override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != LimitState.e_equalLimits)
{
float Cdot = Utilities.Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
float impulse = m_motorMass * (m_motorSpeed - Cdot);
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorForce;
m_motorImpulse = Utilities.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
Vec2 P = impulse * m_axis;
float LA = impulse * m_a1;
float LB = impulse * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
Vec2 Cdot1;
Cdot1.X = Utilities.Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.Y = wB - wA;
if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = Utilities.Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 f1 = m_impulse;
Vec3 df = m_K.Solve33(-Cdot);
m_impulse += df;
if (m_limitState == LimitState.e_atLowerLimit)
{
m_impulse.Z = Math.Max(m_impulse.Z, 0.0f);
}
else if (m_limitState == LimitState.e_atUpperLimit)
{
m_impulse.Z = Math.Min(m_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)
Vec2 b = -Cdot1 - (m_impulse.Z - f1.Z) * new Vec2(m_K.ez.X, m_K.ez.Y);
Vec2 f2r = m_K.Solve22(b) + new Vec2(f1.X, f1.Y);
m_impulse.X = f2r.X;
m_impulse.Y = f2r.Y;
df = m_impulse - f1;
Vec2 P = df.X * m_perp + df.Z * m_axis;
float LA = df.X * m_s1 + df.Y + df.Z * m_a1;
float LB = df.X * m_s2 + df.Y + df.Z * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vec2 df = m_K.Solve22(-Cdot1);
m_impulse.X += df.X;
m_impulse.Y += df.Y;
Vec2 P = df.X * m_perp;
float LA = df.X * m_s1 + df.Y;
float LB = df.X * m_s2 + df.Y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
internal override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 d = cB + rB - cA - rA;
Vec2 axis = Utilities.Mul(qA, m_localXAxisA);
float a1 = Utilities.Cross(d + rA, axis);
float a2 = Utilities.Cross(rB, axis);
Vec2 perp = Utilities.Mul(qA, m_localYAxisA);
float s1 = Utilities.Cross(d + rA, perp);
float s2 = Utilities.Cross(rB, perp);
Vec3 impulse;
Vec2 C1;
C1.X = Utilities.Dot(perp, d);
C1.Y = aB - aA - m_referenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (m_enableLimit)
{
float translation = Utilities.Dot(axis, d);
if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings._linearSlop)
{
// Prevent large angular corrections
C2 = Utilities.Clamp(translation, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Utilities.Clamp(translation - m_lowerTranslation + Settings._linearSlop, -Settings._maxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Utilities.Clamp(translation - m_upperTranslation - Settings._linearSlop, 0.0f, Settings._maxLinearCorrection);
linearError = Math.Max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex.Set(k11, k12, k13);
K.ey.Set(k12, k22, k23);
K.ez.Set(k13, k23, k33);
Vec3 C = new Vec3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex.Set(k11, k12);
K.ey.Set(k12, k22);
Vec2 impulse1 = K.Solve(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vec2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(linearError <= Settings._linearSlop && angularError <= Settings._angularSlop);
}
internal override void InitVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA = m_bodyA.m_sweep.localCenter;
m_localCenterB = m_bodyB.m_sweep.localCenter;
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
// Compute the effective masses.
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 d = (cB - cA) + rB - rA;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
m_axis = Utilities.Mul(qA, m_localXAxisA);
m_a1 = Utilities.Cross(d + rA, m_axis);
m_a2 = Utilities.Cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
m_perp = Utilities.Mul(qA, m_localYAxisA);
m_s1 = Utilities.Cross(d + rA, m_perp);
m_s2 = Utilities.Cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.Set(k11, k12, k13);
m_K.ey.Set(k12, k22, k23);
m_K.ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float jointTranslation = Utilities.Dot(m_axis, d);
if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings._linearSlop)
{
m_limitState = LimitState.e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != LimitState.e_atLowerLimit)
{
m_limitState = LimitState.e_atLowerLimit;
m_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != LimitState.e_atUpperLimit)
{
m_limitState = LimitState.e_atUpperLimit;
m_impulse.Z = 0.0f;
}
}
else
{
m_limitState = LimitState.e_inactiveLimit;
m_impulse.Z = 0.0f;
}
}
else
{
m_limitState = LimitState.e_inactiveLimit;
m_impulse.Z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
Vec2 P = m_impulse.X * m_perp + (m_motorImpulse + m_impulse.Z) * m_axis;
float LA = m_impulse.X * m_s1 + m_impulse.Y + (m_motorImpulse + m_impulse.Z) * m_a1;
float LB = m_impulse.X * m_s2 + m_impulse.Y + (m_motorImpulse + m_impulse.Z) * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
/// Shift the world origin. Useful for large worlds.
/// The shift formula is: position -= newOrigin
/// @param newOrigin the new origin with respect to the old origin
public void ShiftOrigin(Vec2 newOrigin)
{
m_tree.ShiftOrigin(newOrigin);
}
