public ShapeEditing()
{
{
b2BodyDef bd1 = new b2BodyDef();
b2Body ground = m_world.CreateBody(bd1);
b2EdgeShape shape1 = new b2EdgeShape();
shape1.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape1, 0.0f);
}
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(0.0f, 10.0f);
m_body = m_world.CreateBody(bd);
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(4.0f, 4.0f, new b2Vec2(0.0f, 0.0f), 0.0f);
m_fixture1 = m_body.CreateFixture(shape, 10.0f);
m_fixture2 = null;
m_sensor = false;
}
public void Initialize(b2Body bA, b2Body bB,
b2Vec2 gaA, b2Vec2 gaB,
b2Vec2 anchorA, b2Vec2 anchorB,
float r)
{
bodyA = bA;
bodyB = bB;
groundAnchorA.SetV(gaA);
groundAnchorB.SetV(gaB);
localAnchorA = bodyA.GetLocalPoint(anchorA);
localAnchorB = bodyB.GetLocalPoint(anchorB);
//b2Vec2 d1 = anchorA - gaA;
float d1X = anchorA.x - gaA.x;
float d1Y = anchorA.y - gaA.y;
//length1 = d1.Length();
lengthA = Mathf.Sqrt(d1X * d1X + d1Y * d1Y);
//b2Vec2 d2 = anchor2 - ga2;
float d2X = anchorB.x - gaB.x;
float d2Y = anchorB.y - gaB.y;
//length2 = d2.Length();
lengthB = Mathf.Sqrt(d2X * d2X + d2Y * d2Y);
ratio = r;
//b2Settings.b2Assert(ratio > Number.MIN_VALUE);
float C = lengthA + ratio * lengthB;
maxLengthA = C - ratio * b2PulleyJoint.b2_minPulleyLength;
maxLengthB = (C - b2PulleyJoint.b2_minPulleyLength) / ratio;
}
public override void Step(b2TimeStep step)
{
float timestep = step.dt;
if (timestep <= float.MinValue)
{
return;
}
if (timestep > maxTimestep && maxTimestep > 0.0f)
{
timestep = maxTimestep;
}
for (b2ControllerEdge i = m_bodyList; i != null; i = i.nextBody)
{
b2Body body = i.body;
if (!body.IsAwake())
{
//Sleeping bodies are still - so have no damping
continue;
}
b2Vec2 damping =
body.GetWorldVector(
b2Math.MulMV(T,
body.GetLocalVector(
body.GetLinearVelocity()
)
)
);
body.SetLinearVelocity(new b2Vec2(
body.GetLinearVelocity().x + damping.x * timestep,
body.GetLinearVelocity().y + damping.y * timestep
));
}
}
public void Add(b2Body body)
{
Debug.Assert(m_bodyCount < m_bodyCapacity);
body.m_islandIndex = m_bodyCount;
m_bodies[m_bodyCount] = body;
++m_bodyCount;
}
private void testQueryShape_poly()
{
Vector3 mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
b2WorldQueryCallback cb = delegate(b2Fixture fixture) {
b2Body body = fixture.GetBody();
body.SetAwake(true);
return(true);
};
b2Vec2[] vertices = new b2Vec2[4];
vertices [0] = new b2Vec2(-100.0f / ptm_ratio, 0.0f / ptm_ratio);
vertices [1] = new b2Vec2(0.0f / ptm_ratio, 100.0f / ptm_ratio);
vertices [2] = new b2Vec2(100.0f / ptm_ratio, 0.0f / ptm_ratio);
vertices [3] = new b2Vec2(0.0f / ptm_ratio, -200.0f / ptm_ratio);
b2PolygonShape shape = b2PolygonShape.AsArray(vertices, vertices.Length);
b2Transform transform = new b2Transform(new b2Vec2(mousePos.x, mousePos.y), b2Mat22.FromAngle(0));
_world.QueryShape(cb, shape, transform);
for (int i = 0; i < vertices.Length; i++)
{
vertices[i].x += mousePos.x;
vertices[i].y += mousePos.y;
}
b2Color color = new b2Color(1.0f, 0.0f, 0.0f);
_debugDraw.DrawPolygon(vertices, vertices.Length, color);
}
public PolyShapes()
{
// Ground body
{
b2BodyDef bd = new b2BodyDef();
b2Body ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
b2Vec2[] vertices = new b2Vec2[3];
vertices[0].Set(-0.5f, 0.0f);
vertices[1].Set(0.5f, 0.0f);
vertices[2].Set(0.0f, 1.5f);
m_polygons[0] = new b2PolygonShape();
m_polygons[0].Set(vertices, 3);
}
{
b2Vec2[] vertices = new b2Vec2[3];
vertices[0].Set(-0.1f, 0.0f);
vertices[1].Set(0.1f, 0.0f);
vertices[2].Set(0.0f, 1.5f);
m_polygons[1] = new b2PolygonShape();
m_polygons[1].Set(vertices, 3);
}
{
float w = 1.0f;
float b = w / (2.0f + b2Math.b2Sqrt(2.0f));
float s = b2Math.b2Sqrt(2.0f) * b;
b2Vec2[] vertices = new b2Vec2[8];
vertices[0].Set(0.5f * s, 0.0f);
vertices[1].Set(0.5f * w, b);
vertices[2].Set(0.5f * w, b + s);
vertices[3].Set(0.5f * s, w);
vertices[4].Set(-0.5f * s, w);
vertices[5].Set(-0.5f * w, b + s);
vertices[6].Set(-0.5f * w, b);
vertices[7].Set(-0.5f * s, 0.0f);
m_polygons[2] = new b2PolygonShape();
m_polygons[2].Set(vertices, 8);
}
{
m_polygons[3] = new b2PolygonShape();
m_polygons[3].SetAsBox(0.5f, 0.5f);
}
{
m_circle.Radius = 0.5f;
}
m_bodyIndex = 0;
}
void InitPhysics()
{
CCSize size = Layer.VisibleBoundsWorldspace.Size;
var gravity = new b2Vec2(0.0f, -10.0f);
world = new b2World(gravity);
world.SetAllowSleeping(true);
world.SetContinuousPhysics(true);
var def = new b2BodyDef();
def.allowSleep = true;
def.position = b2Vec2.Zero;
def.type = b2BodyType.b2_staticBody;
b2Body groundBody = world.CreateBody(def);
groundBody.SetActive(true);
b2EdgeShape groundBox = new b2EdgeShape();
groundBox.Set(b2Vec2.Zero, new b2Vec2(size.Width / PTM_RATIO, 0));
b2FixtureDef fd = new b2FixtureDef();
fd.shape = groundBox;
groundBody.CreateFixture(fd);
}
public override float ReportFixture(b2Fixture fixture, b2Vec2 point,
b2Vec2 normal, float fraction)
{
b2Body body = fixture.Body;
object userData = body.UserData;
if (userData != null)
{
int index = (int)userData;
if (index == 0)
{
// filter
return(-1.0f);
}
}
Debug.Assert(m_count < e_maxCount);
m_points[m_count] = point;
m_normals[m_count] = normal;
++m_count;
if (m_count == e_maxCount)
{
return(0.0f);
}
return(1.0f);
}
public void AddBody(b2Body body)
{
b2ControllerEdge edge = new b2ControllerEdge();
edge.controller = this;
edge.body = body;
//
edge.nextBody = m_bodyList;
edge.prevBody = null;
m_bodyList = edge;
if (edge.nextBody != null)
{
edge.nextBody.prevBody = edge;
}
m_bodyCount++;
//
edge.nextController = body.m_controllerList;
edge.prevController = null;
body.m_controllerList = edge;
if (edge.nextController != null)
{
edge.nextController.prevController = edge;
}
body.m_controllerCount++;
}
void AddBall()
{
int idx = (CCRandom.Float_0_1() > .5 ? 0 : 1);
int idy = (CCRandom.Float_0_1() > .5 ? 0 : 1);
var sprite = new CCPhysicsSprite(ballTexture, new CCRect(32 * idx, 32 * idy, 32, 32), PTM_RATIO);
ballsBatch.AddChild(sprite);
CCPoint p = GetRandomPosition(sprite.ContentSize);
sprite.Position = new CCPoint(p.X, p.Y);
var def = new b2BodyDef();
def.position = new b2Vec2(p.X / PTM_RATIO, p.Y / PTM_RATIO);
def.linearVelocity = new b2Vec2(0.0f, -1.0f);
def.type = b2BodyType.b2_dynamicBody;
b2Body body = world.CreateBody(def);
var circle = new b2CircleShape();
circle.Radius = 0.5f;
var fd = new b2FixtureDef();
fd.shape = circle;
fd.density = 1f;
fd.restitution = 0.85f;
fd.friction = 0f;
body.CreateFixture(fd);
sprite.PhysicsBody = body;
Console.WriteLine("sprite batch node count = {0}", ballsBatch.ChildrenCount);
}
/// Get the current joint angle in radians.
public float GetJointAngle()
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
return(bB.m_sweep.a - bA.m_sweep.a);
}
protected void mouseDownHandler()
{
Vector3 pos = Camera.main.ScreenToWorldPoint(Input.mousePosition); //屏幕坐标转世界坐标
b2Body b = getPosBody(pos.x, pos.y);
startDragBody(b, pos.x, pos.y);
}
/// 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(b2Body bA, b2Body bB, b2Vec2 groundA, b2Vec2 groundB, b2Vec2 anchorA, b2Vec2 anchorB, float r)
{
bodyA = bA;
bodyB = bB;
groundAnchorA = groundA;
groundAnchorB = groundB;
localAnchorA = bodyA.GetLocalPoint(anchorA);
localAnchorB = bodyB.GetLocalPoint(anchorB);
b2Vec2 dA = anchorA - groundA;
lengthA = dA.Length();
b2Vec2 dB = anchorB - groundB;
lengthB = dB.Length();
ratio = r;
Debug.Assert(ratio > float.Epsilon);
}
public virtual float GetJointSpeed()
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
return(bB.AngularVelocity - bA.AngularVelocity);
}
public b2Body GetBodyB()
{
global::System.IntPtr cPtr = Box2DPINVOKE.b2Joint_GetBodyB(swigCPtr);
b2Body ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Body(cPtr, false);
return(ret);
}
public void Initialize(b2Body bA, b2Body bB, b2Vec2 anchor)
{
BodyA = bA;
BodyB = bB;
localAnchorA = BodyA.GetLocalPoint(anchor);
localAnchorB = BodyB.GetLocalPoint(anchor);
}
// We need separation create/destroy functions from the constructor/destructor because
// the destructor cannot access the allocator (no destructor arguments allowed by C++).
internal void Create(b2Body body, b2FixtureDef def)
{
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
m_next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
m_shape = def.shape.Clone();
// Reserve proxy space
int childCount = m_shape.GetChildCount();
m_proxies = Arrays.InitializeWithDefaultInstances <b2FixtureProxy>(childCount);
for (int i = 0; i < childCount; ++i)
{
m_proxies[i] = new b2FixtureProxy();
m_proxies[i].fixture = null;
m_proxies[i].proxyId = (int)b2BroadPhase.AnonymousEnum.e_nullProxy;
}
m_proxyCount = 0;
m_density = def.density;
}
public b2Body CreateBody(b2BodyDef def)
{
global::System.IntPtr cPtr = Box2dPINVOKE.b2World_CreateBody(swigCPtr, b2BodyDef.getCPtr(def));
b2Body ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Body(cPtr, false);
return(ret);
}
public b2Body GetBodyList()
{
global::System.IntPtr cPtr = Box2dPINVOKE.b2World_GetBodyList__SWIG_0(swigCPtr);
b2Body ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Body(cPtr, false);
return(ret);
}
public void Initialize(b2Body bA, b2Body bB, b2Vec2 anchor)
{
BodyA = bA;
BodyB = bB;
localAnchorA = BodyA.GetLocalPoint(anchor);
localAnchorB = BodyB.GetLocalPoint(anchor);
}
public VaryingRestitution()
{
{
b2BodyDef bd = new b2BodyDef();
b2Body ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
b2CircleShape shape = new b2CircleShape();
shape.Radius = 1.0f;
b2FixtureDef fd = new b2FixtureDef();
fd.shape = shape;
fd.density = 1.0f;
float[] restitution = { 0.0f, 0.1f, 0.3f, 0.5f, 0.75f, 0.9f, 1.0f };
for (int i = 0; i < 7; ++i)
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(-10.0f + 3.0f * i, 20.0f);
b2Body body = m_world.CreateBody(bd);
fd.restitution = restitution[i];
body.CreateFixture(fd);
}
}
}
public virtual float GetJointAngle()
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
return(bB.Sweep.a - bA.Sweep.a - m_referenceAngle);
}
/// Get the current joint angle speed in radians per second.
public float GetJointSpeed()
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
return(bB.m_angularVelocity - bA.m_angularVelocity);
}
/// Get the current joint linear speed, usually in meters per second.
public float GetJointLinearSpeed()
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
b2Vec2 rA = Utils.b2Mul(bA.m_xf.q, m_localAnchorA - bA.m_sweep.localCenter);
b2Vec2 rB = Utils.b2Mul(bB.m_xf.q, m_localAnchorB - bB.m_sweep.localCenter);
b2Vec2 p1 = bA.m_sweep.c + rA;
b2Vec2 p2 = bB.m_sweep.c + rB;
b2Vec2 d = p2 - p1;
b2Vec2 axis = Utils.b2Mul(bA.m_xf.q, m_localXAxisA);
b2Vec2 vA = new b2Vec2(bA.m_linearVelocity);
b2Vec2 vB = new b2Vec2(bB.m_linearVelocity);
float wA = bA.m_angularVelocity;
float wB = bB.m_angularVelocity;
float speed = Utils.b2Dot(d, Utils.b2Cross(wA, axis)) + Utils.b2Dot(axis, vB + Utils.b2Cross(wB, rB) - vA - Utils.b2Cross(wA, rA));
return(speed);
}
private void testQueryAABB()
{
Vector3 mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
b2WorldQueryCallback cb = delegate(b2Fixture fixture) {
b2Body body = fixture.GetBody();
body.SetAwake(true);
return(true);
};
float w = 200 / ptm_ratio;
float h = 200 / ptm_ratio;
b2AABB aabb = b2AABB.MakeWH(w, h, mousePos.x, mousePos.y);
_world.QueryAABB(cb, aabb);
b2Vec2[] vertices = new b2Vec2[] {
new b2Vec2(aabb.lowerBound.x, aabb.lowerBound.y),
new b2Vec2(aabb.upperBound.x, aabb.lowerBound.y),
new b2Vec2(aabb.upperBound.x, aabb.upperBound.y),
new b2Vec2(aabb.lowerBound.x, aabb.upperBound.y)
};
b2Color color = new b2Color(1.0f, 0.0f, 0.0f);
_debugDraw.DrawPolygon(vertices, vertices.Length, color);
}
public Breakable()
{
// Ground body
{
b2BodyDef bd = new b2BodyDef();
b2Body ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
// Breakable dynamic body
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position = new b2Vec2(0.0f, 40.0f);
bd.angle = 0.25f * b2Settings.b2_pi;
m_body1 = m_world.CreateBody(bd);
m_shape1.SetAsBox(0.5f, 0.5f, new b2Vec2(-0.5f, 0.0f), 0.0f);
m_piece1 = m_body1.CreateFixture(m_shape1, 1.0f);
m_shape2.SetAsBox(0.5f, 0.5f, new b2Vec2(0.5f, 0.0f), 0.0f);
m_piece2 = m_body1.CreateFixture(m_shape2, 1.0f);
}
m_break = false;
m_broke = false;
}
public SphereStack()
{
{
b2BodyDef bd = new b2BodyDef();
b2Body ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
b2CircleShape shape = new b2CircleShape();
shape.Radius = 1.0f;
for (int i = 0; i < e_count; ++i)
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(0.0f, 4.0f + 3.0f * i);
m_bodies[i] = m_world.CreateBody(bd);
m_bodies[i].CreateFixture(shape, 1.0f);
m_bodies[i].LinearVelocity = new b2Vec2(0.0f, -50.0f);
}
}
}
public Test()
{
m_destructionListener = new DestructionListener();
m_debugDraw = new CCBox2dDraw("fonts/arial-12");
b2Vec2 gravity = new b2Vec2();
gravity.Set(0.0f, -10.0f);
m_world = new b2World(gravity);
m_bomb = null;
m_textLine = 30;
m_mouseJoint = null;
m_pointCount = 0;
m_destructionListener.test = this;
m_world.SetDestructionListener(m_destructionListener);
m_world.SetContactListener(this);
m_world.SetDebugDraw(m_debugDraw);
m_world.SetContinuousPhysics(true);
m_world.SetWarmStarting(true);
m_bombSpawning = false;
m_stepCount = 0;
b2BodyDef bodyDef = new b2BodyDef();
m_groundBody = m_world.CreateBody(bodyDef);
}
public b2Body GetBody()
{
global::System.IntPtr cPtr = Box2DPINVOKE.b2Fixture_GetBody__SWIG_0(swigCPtr);
b2Body ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Body(cPtr, false);
return(ret);
}
public override void Step(Settings settings)
{
base.Step(settings);
// Traverse the contact results. Apply a force on shapes
// that overlap the sensor.
for (int i = 0; i < e_count; ++i)
{
if (m_touching[i] == false)
{
continue;
}
b2Body body = m_bodies[i];
b2Body ground = m_sensor.Body;
b2CircleShape circle = (b2CircleShape)m_sensor.Shape;
b2Vec2 center = ground.GetWorldPoint(circle.Position);
b2Vec2 position = body.Position;
b2Vec2 d = center - position;
if (d.LengthSquared < float.Epsilon * float.Epsilon)
{
continue;
}
d.Normalize();
b2Vec2 F = 100.0f * d;
body.ApplyForce(F, position);
}
}
void InitPhysics()
{
var gravity = new b2Vec2(0.0f, -10.0f);
world = new b2World(gravity);
world.SetAllowSleeping(true);
world.SetContinuousPhysics(true);
var def = new b2BodyDef();
def.allowSleep = true;
def.position = b2Vec2.Zero;
def.type = b2BodyType.b2_staticBody;
b2Body groundBody = world.CreateBody(def);
groundBody.SetActive(true);
b2EdgeShape groundBox = new b2EdgeShape();
groundBox.Set(b2Vec2.Zero, new b2Vec2(900, 100));
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 0.3f;
fd.restitution = 0.1f;
fd.shape = groundBox;
groundBody.CreateFixture(fd);
}
/// Initialize the bodies, anchors, and reference angle using a world
/// anchor point.
public void Initialize(b2Body bA, b2Body bB, b2Vec2 anchor)
{
BodyA = bA;
BodyB = bB;
localAnchorA = BodyA.GetLocalPoint(anchor);
localAnchorB = BodyB.GetLocalPoint(anchor);
referenceAngle = BodyB.Angle - BodyA.Angle;
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
public void Initialize(b2Body bA, b2Body bB,
b2Vec2 groundA, b2Vec2 groundB,
b2Vec2 anchorA, b2Vec2 anchorB,
float r)
{
BodyA = bA;
BodyB = bB;
groundAnchorA = groundA;
groundAnchorB = groundB;
localAnchorA = BodyA.GetLocalPoint(anchorA);
localAnchorB = BodyB.GetLocalPoint(anchorB);
b2Vec2 dA = anchorA - groundA;
lengthA = dA.Length();
b2Vec2 dB = anchorB - groundB;
lengthB = dB.Length();
ratio = r;
System.Diagnostics.Debug.Assert(ratio > b2Settings.b2_epsilon);
}
public b2Joint(b2JointDef def)
{
Debug.Assert(def.BodyA != def.BodyB);
m_type = def.JointType;
Prev = null;
Next = null;
m_bodyA = def.BodyA;
m_bodyB = def.BodyB;
m_index = 0;
m_collideConnected = def.CollideConnected;
m_islandFlag = false;
m_userData = def.UserData;
m_edgeA.Joint = null;
m_edgeA.Other = null;
m_edgeA.Prev = null;
m_edgeA.Next = null;
m_edgeB.Joint = null;
m_edgeB.Other = null;
m_edgeB.Prev = null;
m_edgeB.Next = null;
}
public b2GearJoint(b2GearJointDef def)
: base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetJointType();
m_typeB = m_joint2.GetJointType();
Debug.Assert(m_typeA == b2JointType.e_revoluteJoint || m_typeA == b2JointType.e_prismaticJoint);
Debug.Assert(m_typeB == b2JointType.e_revoluteJoint || m_typeB == b2JointType.e_prismaticJoint);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1.GetBodyA();
m_bodyA = m_joint1.GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA.XF;
float aA = m_bodyA.Sweep.a;
b2Transform xfC = m_bodyC.XF;
float aC = m_bodyC.Sweep.a;
if (m_typeA == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint1;
m_localAnchorC = revolute.GetLocalAnchorA();
m_localAnchorA = revolute.GetLocalAnchorB();
m_referenceAngleA = revolute.GetReferenceAngle();
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.GetLocalAnchorA();
m_localAnchorA = prismatic.GetLocalAnchorB();
m_referenceAngleA = prismatic.GetReferenceAngle();
m_localAxisC = prismatic.GetLocalXAxisA();
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2Math.b2MulT(xfC.q, b2Math.b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Math.b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2.GetBodyA();
m_bodyB = m_joint2.GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB.XF;
float aB = m_bodyB.Sweep.a;
b2Transform xfD = m_bodyD.XF;
float aD = m_bodyD.Sweep.a;
if (m_typeB == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint2;
m_localAnchorD = revolute.GetLocalAnchorA();
m_localAnchorB = revolute.GetLocalAnchorB();
m_referenceAngleB = revolute.GetReferenceAngle();
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.GetLocalAnchorA();
m_localAnchorB = prismatic.GetLocalAnchorB();
m_referenceAngleB = prismatic.GetReferenceAngle();
m_localAxisD = prismatic.GetLocalXAxisA();
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2Math.b2MulT(xfD.q, b2Math.b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Math.b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
