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