// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
static int32 ProcessThree(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
{
b2Vec2 a = points[0];
b2Vec2 b = points[1];
b2Vec2 c = points[2];
b2Vec2 ab = b - a;
b2Vec2 ac = c - a;
b2Vec2 bc = c - b;
float32 sn = -b2Dot(a, ab), sd = b2Dot(b, ab);
float32 tn = -b2Dot(a, ac), td = b2Dot(c, ac);
float32 un = -b2Dot(b, bc), ud = b2Dot(c, bc);
// In vertex c region?
if (td <= 0.0f && ud <= 0.0f)
{
// Single point
*x1 = p1s[2];
*x2 = p2s[2];
p1s[0] = p1s[2];
p2s[0] = p2s[2];
points[0] = points[2];
return 1;
}
// Should not be in vertex a or b region.
B2_NOT_USED(sd);
B2_NOT_USED(sn);
b2Assert(sn > 0.0f || tn > 0.0f);
b2Assert(sd > 0.0f || un > 0.0f);
float32 n = b2Cross(ab, ac);
#ifdef TARGET_FLOAT32_IS_FIXED
n = (n < 0.0)? -1.0 : ((n > 0.0)? 1.0 : 0.0);
#endif
// Should not be in edge ab region.
float32 vc = n * b2Cross(a, b);
b2Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);
// In edge bc region?
float32 va = n * b2Cross(b, c);
if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un+ud) > 0.0f)
{
b2Assert(un + ud > 0.0f);
float32 lambda = un / (un + ud);
*x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
*x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
p1s[0] = p1s[2];
p2s[0] = p2s[2];
points[0] = points[2];
return 2;
}
// In edge ac region?
float32 vb = n * b2Cross(c, a);
if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn+td) > 0.0f)
{
b2Assert(tn + td > 0.0f);
float32 lambda = tn / (tn + td);
*x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
*x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
p1s[1] = p1s[2];
p2s[1] = p2s[2];
points[1] = points[2];
return 2;
}
// Inside the triangle, compute barycentric coordinates
float32 denom = va + vb + vc;
b2Assert(denom > 0.0f);
denom = 1.0f / denom;
#ifdef TARGET_FLOAT32_IS_FIXED
*x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
*x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
#else
float32 u = va * denom;
float32 v = vb * denom;
float32 w = 1.0f - u - v;
*x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
*x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
#endif
return 3;
}
bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 s1 = m_groundAnchor1;
b2Vec2 s2 = m_groundAnchor2;
b2Vec2 r1 = b2Mul(b1->m_xf.R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->m_xf.R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
// Get the pulley axes.
b2Vec2 u1 = p1 - s1;
b2Vec2 u2 = p2 - s2;
float32 length1 = u1.Length();
float32 length2 = u2.Length();
if (length1 > 10.0f * b2_linearSlop)
{
u1 *= 1.0f / length1;
}
else
{
u1.SetZero();
}
if (length2 > 10.0f * b2_linearSlop)
{
u2 *= 1.0f / length2;
}
else
{
u2.SetZero();
}
// Compute effective mass.
float32 cr1u1 = b2Cross(r1, u1);
float32 cr2u2 = b2Cross(r2, u2);
float32 m1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
float32 m2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
float32 mass = m1 + m_ratio * m_ratio * m2;
if (mass > 0.0f)
{
mass = 1.0f / mass;
}
float32 C = m_constant - length1 - m_ratio * length2;
float32 linearError = b2Abs(C);
float32 impulse = -mass * C;
b2Vec2 P1 = -impulse * u1;
b2Vec2 P2 = -m_ratio * impulse * u2;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
return linearError < b2_linearSlop;
}
// Default implementation of b2AllocFunction.
static void* b2AllocDefault(int32 size, void* callbackData)
{
B2_NOT_USED(callbackData);
return malloc(size);
}
virtual void post_solve(const b2Contact* contact, const b2ContactImpulse* impulse)
{
B2_NOT_USED(contact);
B2_NOT_USED(impulse);
}
bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
{
B2_NOT_USED(xf);
B2_NOT_USED(p);
return false;
}
void mouse_move(const b2Vec2& p) { B2_NOT_USED(p); }
// Callbacks for derived classes.
virtual void begin_contact(b2Contact* contact) { B2_NOT_USED(contact); }
float32 b2ElasticRopeJoint::GetReactionTorque(float32 inv_dt) const
{
B2_NOT_USED(inv_dt);
return 0.0f;
}
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
// Set up the scene
QGraphicsScene scene;
QRect sceneRect(0,0,740,540);
scene.setSceneRect(sceneRect);
scene.setItemIndexMethod(QGraphicsScene::NoIndex);
Team A(5,QColor(0,0,255));
Team B(5,QColor(255,255,0),50);
A.addToScene(scene);
B.addToScene(scene);
// Set up the view port
MyGraphicsView v;
v.setRenderHint(QPainter::Antialiasing);
v.setCacheMode(QGraphicsView::CacheBackground);
v.setViewportUpdateMode(QGraphicsView::BoundingRectViewportUpdate);
v.setDragMode(QGraphicsView::ScrollHandDrag);
v.setScene(&scene);
v.show();
// Call circle advance for every time interval of 1000/33
QTimer timer;
QObject::connect(&timer, SIGNAL(timeout()), &scene, SLOT(advance()));
timer.start(1000 / 33);
a.exec();
#ifndef __NO_BOX2D__
B2_NOT_USED(argc);
B2_NOT_USED(argv);
// Define the ground body.
b2BodyDef groundBodyDef[4];
groundBodyDef[0].position.Set(0.0f, 2.7f); //top
groundBodyDef[1].position.Set(0.0f, -2.7f); //bottom
groundBodyDef[2].position.Set(-3.7f, 0.0f); //left
groundBodyDef[3].position.Set(3.7f, 0.0f); //right
// 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.
b2Body* groundBody[4];
for(unsigned int i = 0; i < 4; i += 1){
groundBody[i] = m_world->CreateBody(&groundBodyDef[i]);
}
// Define the ground box shape.
b2PolygonShape groundBox[4];
// The extents are the half-widths of the box.
groundBox[0].SetAsBox(3.7f, 0.003f);
groundBox[1].SetAsBox(3.7f, 0.003f);
groundBox[2].SetAsBox(0.003f, 2.7f);
groundBox[3].SetAsBox(0.003f, 2.7f);
// Add the ground fixture to the ground body.
for(unsigned int i = 0; i < 4; i += 1){
groundBody[i]->CreateFixture(&groundBox[i], 0.0f);
}
// Define the dynamic body. We set its position and call the body factory.
b2BodyDef bodyDef[6];
for(unsigned int i = 0; i < 6; i += 1){
bodyDef[i].type = b2_dynamicBody;
}
bodyDef[0].position.Set(1.0f, 2.0f);
bodyDef[1].position.Set(1.0f, 0.0f);
bodyDef[2].position.Set(1.0f, -2.0f);
bodyDef[3].position.Set(2.0f, 1.0f);
bodyDef[4].position.Set(2.0f, -1.0f);
bodyDef[5].position.Set(3.0f, 0.0f);
b2Body* body[6];
for(unsigned int i = 0; i < 6; i += 1){
body[i] = m_world->CreateBody(&bodyDef[i]);
b2Vec2 pos = body[i]->GetPosition();
body[i]->SetTransform(pos, 90.0);
}
// Define another box shape for our dynamic body.
b2PolygonShape dynamicBox;
dynamicBox.SetAsBox(0.09f, 0.09f);
b2Vec2 vertices[19];
vertices[0].Set(-0.090000f, 0.000000f);
vertices[1].Set(-0.087385f, -0.021538f);
vertices[2].Set(-0.079691f, -0.041825f);
vertices[3].Set(-0.067366f, -0.059681f);
vertices[4].Set(-0.051126f, -0.074069f);
vertices[5].Set(-0.031914f, -0.084151f);
vertices[6].Set(-0.010848f, -0.089344f);
vertices[7].Set(0.010848f, -0.089344f);
vertices[8].Set(0.031914f, -0.084151f);
vertices[9].Set(0.051126f, -0.074069f);
vertices[10].Set(0.067366f, -0.059681f);
vertices[11].Set(0.079691f, -0.041825f);
vertices[12].Set(0.087385f, -0.021538f);
vertices[13].Set(0.087385f, 0.021538f);
vertices[14].Set(0.079691f, 0.041825f);
vertices[15].Set(0.067366f, 0.059681f);
vertices[16].Set(-0.067366f, 0.059681f);
vertices[17].Set(-0.079691f, 0.041825f);
vertices[18].Set(-0.087385f, 0.021538f);
int32 count = 19;
b2PolygonShape polygon;
polygon.Set(vertices, count);
// Define the dynamic body fixture.
b2FixtureDef fixtureDef;
b2FixtureDef fixtureDefTri;
fixtureDef.shape = &dynamicBox;
fixtureDefTri.shape = &polygon;
// Set the box density to be non-zero, so it will be dynamic.
fixtureDef.density = 1.0f;
fixtureDefTri.density = 1.0f;
// Override the default friction.
fixtureDef.friction = 0.3f;
fixtureDefTri.friction = 0.3f;
// Add the shape to the body.
for(unsigned int i = 0; i < 6; i += 1){
body[i]->CreateFixture(&fixtureDefTri);
}
// Prepare for simulation. Typically we use a time step of 1/60 of a
// second (60Hz) and 10 iterations. This provides a high quality simulation
// in most game scenarios.
float32 timeStep = 1.0f / 60.0f;
int32 velocityIterations = 6;
int32 positionIterations = 2;
// This is our little game loop.
for (int32 i = 0; i < 60; ++i)
{
// Instruct the world to perform a single step of simulation.
// It is generally best to keep the time step and iterations fixed.
world.Step(timeStep, velocityIterations, positionIterations);
b2Vec2 position[6];
float32 angle[6];
// Now print the position and angle of the body.
for(unsigned int i = 0; i < 6; i += 1){
position[i] = body[i]->GetPosition();
angle[i] = body[i]->GetAngle();
}
printf("----------------------------------\n");
for(unsigned int i = 0; i < 5; i += 1){
printf("%4.2f %4.2f\n", (position[i].x + 2.7) * 100, 540 - ((position[i].y + 3.7) * 100));
A.setPos(i, (position[i].x + 2.7) * 100, 540 - ((position[i].y + 3.7) * 100));
}
printf("----------------------------------\n");
}
#endif
// When the world destructor is called, all bodies and joints are freed. This can
// create orphaned pointers, so be careful about your world management.
return 0;
}
qreal b2RopeJoint::GetReactionTorque(qreal inv_dt) const
{
B2_NOT_USED(inv_dt);
return 0.0f;
}
bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte)
{
// TODO_ERIN block solve with limit.
B2_NOT_USED(baumgarte);
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
float32 angularError = 0.0f;
float32 positionError = 0.0f;
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
float32 limitImpulse = 0.0f;
if (m_limitState == e_equalLimits)
{
// Prevent large angular corrections
float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Abs(C);
}
else if (m_limitState == e_atLowerLimit)
{
float32 C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == e_atUpperLimit)
{
float32 C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
b1->m_sweep.a -= b1->m_invI * limitImpulse;
b2->m_sweep.a += b2->m_invI * limitImpulse;
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
// Solve point-to-point constraint.
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
positionError = C.Length();
float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
// Handle large detachment.
const float32 k_allowedStretch = 10.0f * b2_linearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
b2Vec2 u = C; u.Normalize();
float32 k = invMass1 + invMass2;
b2Assert(k > B2_FLT_EPSILON);
float32 m = 1.0f / k;
b2Vec2 impulse = m * (-C);
const float32 k_beta = 0.5f;
b1->m_sweep.c -= k_beta * invMass1 * impulse;
b2->m_sweep.c += k_beta * invMass2 * impulse;
C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
}
b2Mat22 K1;
K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
b2Mat22 K2;
K2.col1.x = invI1 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y;
K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x;
b2Mat22 K3;
K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y;
K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x;
b2Mat22 K = K1 + K2 + K3;
b2Vec2 impulse = K.Solve(-C);
b1->m_sweep.c -= b1->m_invMass * impulse;
b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
b2->m_sweep.c += b2->m_invMass * impulse;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
virtual void PreSolve(b2Contact *contact, const b2Manifold *oldManifold)
{
B2_NOT_USED(contact);
B2_NOT_USED(oldManifold);
}
virtual void EndContact(b2Contact *contact)
{
//! clear list at end
contact_list.clear();
B2_NOT_USED(contact);
}
// This is a simple example of building and running a simulation
// using Box2D. Here we create a large ground box and a small dynamic
// box.
int main(int argc, char** argv)
{
B2_NOT_USED(argc);
B2_NOT_USED(argv);
// Define the size of the world. Simulation will still work
// if bodies reach the end of the world, but it will be slower.
b2AABB worldAABB;
worldAABB.lowerBound.Set(-100.0f, -100.0f);
worldAABB.upperBound.Set(100.0f, 100.0f);
// Define the gravity vector.
b2Vec2 gravity(0.0f, -10.0f);
// Do we want to let bodies sleep?
bool doSleep = true;
// Construct a world object, which will hold and simulate the rigid bodies.
b2World world(worldAABB, gravity, doSleep);
// Define the ground body.
b2BodyDef groundBodyDef;
groundBodyDef.position.Set(0.0f, -10.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.
b2Body* groundBody = world.CreateBody(&groundBodyDef);
// Define the ground box shape.
b2PolygonDef groundShapeDef;
// The extents are the half-widths of the box.
groundShapeDef.SetAsBox(50.0f, 10.0f);
// Add the ground shape to the ground body.
groundBody->CreateFixture(&groundShapeDef);
// Define the dynamic body. We set its position and call the body factory.
b2BodyDef bodyDef;
bodyDef.position.Set(0.0f, 4.0f);
b2Body* body = world.CreateBody(&bodyDef);
// Define another box shape for our dynamic body.
b2PolygonDef shapeDef;
shapeDef.SetAsBox(1.0f, 1.0f);
// Set the box density to be non-zero, so it will be dynamic.
shapeDef.density = 1.0f;
// Override the default friction.
shapeDef.friction = 0.3f;
// Add the shape to the body.
body->CreateFixture(&shapeDef);
// Now tell the dynamic body to compute it's mass properties base
// on its shape.
body->SetMassFromShapes();
// Prepare for simulation. Typically we use a time step of 1/60 of a
// second (60Hz) and 10 iterations. This provides a high quality simulation
// in most game scenarios.
float32 timeStep = 1.0f / 60.0f;
int32 velocityIterations = 8;
int32 positionIterations = 1;
// This is our little game loop.
for (int32 i = 0; i < 60; ++i)
{
// Instruct the world to perform a single step of simulation. It is
// generally best to keep the time step and iterations fixed.
world.Step(timeStep, velocityIterations, positionIterations);
// Now print the position and angle of the body.
b2Vec2 position = body->GetPosition();
float32 angle = body->GetAngle();
printf("%4.2f %4.2f %4.2f\n", position.x, position.y, angle);
}
// When the world destructor is called, all bodies and joints are freed. This can
// create orphaned pointers, so be careful about your world management.
return 0;
}
void shift_mouse_down(const b2Vec2& p) { B2_NOT_USED(p); }
bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data)
{
B2_NOT_USED(data);
return true;
}
virtual void mouse_up(const b2Vec2& p) { B2_NOT_USED(p); }
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
float32 lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->fraction = lower;
output->normal = b2Mul(xf.q, m_normals[index]);
return true;
}
return false;
}
// Let derived tests know that a joint was destroyed.
virtual void joint_destroyed(b2Joint* joint) { B2_NOT_USED(joint); }
bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 c1 = b1->m_sweep.c;
float32 a1 = b1->m_sweep.a;
b2Vec2 c2 = b2->m_sweep.c;
float32 a2 = b2->m_sweep.a;
// Solve linear limit constraint.
float32 linearError = 0.0f, angularError = 0.0f;
bool active = false;
float32 C2 = 0.0f;
b2Mat22 R1(a1), R2(a2);
b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenter1);
b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenter2);
b2Vec2 d = c2 + r2 - c1 - r1;
if (m_enableLimit)
{
m_axis = b2Mul(R1, m_localXAxis1);
m_a1 = b2Cross(d + r1, m_axis);
m_a2 = b2Cross(r2, m_axis);
float32 translation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
linearError = b2Abs(translation);
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
linearError = m_lowerTranslation - translation;
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
linearError = translation - m_upperTranslation;
active = true;
}
}
m_perp = b2Mul(R1, m_localYAxis1);
m_s1 = b2Cross(d + r1, m_perp);
m_s2 = b2Cross(r2, m_perp);
b2Vec3 impulse;
b2Vec2 C1;
C1.x = b2Dot(m_perp, d);
C1.y = a2 - a1 - m_refAngle;
linearError = b2Max(linearError, b2Abs(C1.x));
angularError = b2Abs(C1.y);
if (active)
{
float32 m1 = m_invMass1, m2 = m_invMass2;
float32 i1 = m_invI1, i2 = m_invI2;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 k12 = i1 * m_s1 + i2 * m_s2;
float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
float32 k22 = i1 + i2;
float32 k23 = i1 * m_a1 + i2 * m_a2;
float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
m_K.col1.Set(k11, k12, k13);
m_K.col2.Set(k12, k22, k23);
m_K.col3.Set(k13, k23, k33);
b2Vec3 C;
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = m_K.Solve33(-C);
}
else
{
float32 m1 = m_invMass1, m2 = m_invMass2;
float32 i1 = m_invI1, i2 = m_invI2;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 k12 = i1 * m_s1 + i2 * m_s2;
float32 k22 = i1 + i2;
m_K.col1.Set(k11, k12, 0.0f);
m_K.col2.Set(k12, k22, 0.0f);
b2Vec2 impulse1 = m_K.Solve22(-C1);
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
b2Vec2 P = impulse.x * m_perp + impulse.z * m_axis;
float32 L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1;
float32 L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2;
c1 -= m_invMass1 * P;
a1 -= m_invI1 * L1;
c2 += m_invMass2 * P;
a2 += m_invI2 * L2;
// TODO_ERIN remove need for this.
b1->m_sweep.c = c1;
b1->m_sweep.a = a1;
b2->m_sweep.c = c2;
b2->m_sweep.a = a2;
b1->SynchronizeTransform();
b2->SynchronizeTransform();
return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
virtual void end_contact(b2Contact* contact) { B2_NOT_USED(contact); }
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 iA = vc->invIA;
float32 mB = vc->invMassB;
float32 iB = vc->invIB;
int32 pointCount = vc->pointCount;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Vec2 normal = vc->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 friction = vc->friction;
b2Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
float32 lambda = vcp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * vcp->normalImpulse;
float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp->tangentImpulse;
vcp->tangentImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
// Solve normal constraints
if (pointCount == 1 || g_blockSolve == false)
{
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - vcp->normalImpulse;
vcp->normalImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
b2VelocityConstraintPoint* cp1 = vc->points + 0;
b2VelocityConstraintPoint* cp2 = vc->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
// Compute b'
b -= b2Mul(vc->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x + b'
//
// Solve for x:
//
// x = - inv(A) * b'
//
b2Vec2 x = - b2Mul(vc->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1 + a12 * 0 + b1'
// vn2 = a21 * x1 + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = vc->K.ex.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2 + b1'
// 0 = a21 * 0 + a22 * x2 + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = vc->K.ey.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
m_velocities[indexA].v = vA;
m_velocities[indexA].w = wA;
m_velocities[indexB].v = vB;
m_velocities[indexB].w = wB;
}
}
void Keyboard(unsigned char key, int x, int y)
{
B2_NOT_USED(x);
B2_NOT_USED(y);
switch (key)
{
case 27:
exit(0);
break;
// Press 'z' to zoom out.
case 'z':
viewZoom = b2Min(1.1f * viewZoom, 20.0f);
Resize(width, height);
break;
// Press 'x' to zoom in.
case 'x':
viewZoom = b2Max(0.9f * viewZoom, 0.02f);
Resize(width, height);
break;
// Press 'r' to reset.
case 'r':
delete test;
test = entry->createFcn();
break;
// Press space to launch a bomb.
case ' ':
if (test)
{
test->LaunchBomb();
}
break;
case 'p':
settings.pause = !settings.pause;
break;
// Press [ to prev test.
case '[':
--testSelection;
if (testSelection < 0)
{
testSelection = testCount - 1;
}
glui->sync_live();
break;
// Press ] to next test.
case ']':
++testSelection;
if (testSelection == testCount)
{
testSelection = 0;
}
glui->sync_live();
break;
default:
if (test)
{
test->Keyboard(key);
}
}
}
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* bodyA = c->bodyA;
b2Body* bodyB = c->bodyB;
float32 wA = bodyA->m_angularVelocity;
float32 wB = bodyB->m_angularVelocity;
b2Vec2 vA = bodyA->m_linearVelocity;
b2Vec2 vB = bodyB->m_linearVelocity;
float32 invMassA = bodyA->m_invMass;
float32 invIA = bodyA->m_invI;
float32 invMassB = bodyB->m_invMass;
float32 invIB = bodyB->m_invI;
b2Vec2 normal = c->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 convspeed1 = c->fixtureA->GetConveyorSpeed();
float32 convspeed2 = c->fixtureB->GetConveyorSpeed();
float32 friction = c->friction;
b2Assert(c->pointCount == 1 || c->pointCount == 2);
// Solve tangent constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2ContactConstraintPoint* ccp = c->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent);
/// Conveyor
int flip = (c->manifold->type == b2Manifold::e_faceB ? -1 : 1);
vt += convspeed1 * flip;
vt -= convspeed2 * flip;
/// END Conveyor
float32 lambda = ccp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * ccp->normalImpulse;
float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->tangentImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * b2Cross(ccp->rA, P);
vB += invMassB * P;
wB += invIB * b2Cross(ccp->rB, P);
ccp->tangentImpulse = newImpulse;
}
// Solve normal constraints
if (c->pointCount == 1)
{
b2ContactConstraintPoint* ccp = c->points + 0;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp->normalImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * b2Cross(ccp->rA, P);
vB += invMassB * P;
wB += invIB * b2Cross(ccp->rB, P);
ccp->normalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
b2ContactConstraintPoint* cp1 = c->points + 0;
b2ContactConstraintPoint* cp2 = c->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
b -= b2Mul(c->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
b2Vec2 x = - b2Mul(c->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = c->K.col1.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = c->K.col2.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
bodyA->m_linearVelocity = vA;
bodyA->m_angularVelocity = wA;
bodyB->m_linearVelocity = vB;
bodyB->m_angularVelocity = wB;
}
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
b2Vec2 v2 = m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float32 numerator = b2Dot(normal, v1 - p1);
float32 denominator = b2Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float32 t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return false;
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float32 rr = b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float32 s = b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output->fraction = t;
if (numerator > 0.0f)
{
output->normal = -b2Mul(xf.q, normal);
}
else
{
output->normal = b2Mul(xf.q, normal);
}
return true;
}
bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
float32 linearError = 0.0f;
if (m_state == e_atUpperLimit)
{
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
// Get the pulley axes.
m_u1 = p1 - s1;
m_u2 = p2 - s2;
float32 length1 = m_u1.Length();
float32 length2 = m_u2.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_constant - length1 - m_ratio * length2;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_pulleyMass * C;
b2Vec2 P1 = -impulse * m_u1;
b2Vec2 P2 = -m_ratio * impulse * m_u2;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
if (m_limitState1 == e_atUpperLimit)
{
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
m_u1 = p1 - s1;
float32 length1 = m_u1.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
float32 C = m_maxLength1 - length1;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_limitMass1 * C;
b2Vec2 P1 = -impulse * m_u1;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b1->SynchronizeTransform();
}
if (m_limitState2 == e_atUpperLimit)
{
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p2 = b2->m_sweep.c + r2;
m_u2 = p2 - s2;
float32 length2 = m_u2.Length();
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_maxLength2 - length2;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_limitMass2 * C;
b2Vec2 P2 = -impulse * m_u2;
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b2->SynchronizeTransform();
}
return linearError < b2_linearSlop;
}
float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
{
B2_NOT_USED(inv_dt);
return 0.0f;
}
virtual void keyboard_up(unsigned char key) { B2_NOT_USED(key); }
// Default implementation of b2FreeFunction.
static void b2FreeDefault(void* mem, void* callbackData)
{
B2_NOT_USED(callbackData);
free(mem);
}
bool b2FrictionJoint::SolvePositionConstraints(qreal baumgarte)
{
B2_NOT_USED(baumgarte);
return true;
}
