void* b2StackAllocator::Allocate(int32 size) { b2Assert(m_entryCount < b2_maxStackEntries); b2StackEntry* entry = m_entries + m_entryCount; entry->size = size; if (m_index + size > b2_stackSize) { entry->data = (char*)b2Alloc(size); entry->usedMalloc = true; } else { entry->data = m_data + m_index; entry->usedMalloc = false; m_index += size; } m_allocation += size; m_maxAllocation = b2Max(m_maxAllocation, m_allocation); ++m_entryCount; return entry->data; }
// This is called from b2DynamicTree::Query when we are gathering pairs. bool b2BroadPhase::QueryCallback(int32 proxyId) { // A proxy cannot form a pair with itself. if (proxyId == m_queryProxyId) { return true; } // Grow the pair buffer as needed. if (m_pairCount == m_pairCapacity) { b2Pair* oldBuffer = m_pairBuffer; m_pairCapacity *= 2; m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); b2Free(oldBuffer); } m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); ++m_pairCount; return true; }
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 b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step) { b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; b2Vec2 v1 = b1->m_linearVelocity; float32 w1 = b1->m_angularVelocity; b2Vec2 v2 = b2->m_linearVelocity; float32 w2 = b2->m_angularVelocity; // Solve linear motor constraint. if (m_enableMotor && m_limitState != e_equalLimits) { float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; float32 impulse = m_motorMass * (m_motorSpeed - Cdot); float32 oldImpulse = m_motorImpulse; float32 maxImpulse = step.dt * m_maxMotorForce; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; b2Vec2 P = impulse * m_axis; float32 L1 = impulse * m_a1; float32 L2 = impulse * m_a2; v1 -= m_invMassA * P; w1 -= m_invIA * L1; v2 += m_invMassB * P; w2 += m_invIB * L2; } float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1; if (m_enableLimit && m_limitState != e_inactiveLimit) { // Solve prismatic and limit constraint in block form. float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; b2Vec2 Cdot(Cdot1, Cdot2); b2Vec2 f1 = m_impulse; b2Vec2 df = m_K.Solve(-Cdot); m_impulse += df; if (m_limitState == e_atLowerLimit) { m_impulse.y = b2Max(m_impulse.y, 0.0f); } else if (m_limitState == e_atUpperLimit) { m_impulse.y = b2Min(m_impulse.y, 0.0f); } // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x; float32 f2r; if (m_K.col1.x != 0.0f) { f2r = b / m_K.col1.x + f1.x; } else { f2r = f1.x; } m_impulse.x = f2r; df = m_impulse - f1; b2Vec2 P = df.x * m_perp + df.y * m_axis; float32 L1 = df.x * m_s1 + df.y * m_a1; float32 L2 = df.x * m_s2 + df.y * m_a2; v1 -= m_invMassA * P; w1 -= m_invIA * L1; v2 += m_invMassB * P; w2 += m_invIB * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. float32 df; if (m_K.col1.x != 0.0f) { df = - Cdot1 / m_K.col1.x; } else { df = 0.0f; } m_impulse.x += df; b2Vec2 P = df * m_perp; float32 L1 = df * m_s1; float32 L2 = df * m_s2; v1 -= m_invMassA * P; w1 -= m_invIA * L1; v2 += m_invMassB * P; w2 += m_invIB * L2; } b1->m_linearVelocity = v1; b1->m_angularVelocity = w1; b2->m_linearVelocity = v2; b2->m_angularVelocity = w2; }
void b2Distance(b2DistanceOutput* output, b2SimplexCache* cache, const b2DistanceInput* input) { ++b2_gjkCalls; const b2DistanceProxy* proxyA = &input->proxyA; const b2DistanceProxy* proxyB = &input->proxyB; b2Transform transformA = input->transformA; b2Transform transformB = input->transformB; // Initialize the simplex. b2Simplex simplex; simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. b2SimplexVertex* vertices = &simplex.m_v1; const int32 k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. int32 saveA[3], saveB[3]; int32 saveCount = 0; b2Vec2 closestPoint = simplex.GetClosestPoint(); float32 distanceSqr1 = closestPoint.LengthSquared(); float32 distanceSqr2 = distanceSqr1; // Main iteration loop. int32 iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int32 i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: b2Assert(false); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.m_count == 3) { break; } // Compute closest point. b2Vec2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. b2Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < b2_epsilon * b2_epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. b2SimplexVertex* vertex = vertices + simplex.m_count; vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); b2Vec2 wBLocal; vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; // Iteration count is equated to the number of support point calls. ++iter; ++b2_gjkIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int32 i = 0; i < saveCount; ++i) { if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.m_count; } b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(&output->pointA, &output->pointB); output->distance = b2Distance(output->pointA, output->pointB); output->iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input->useRadii) { float32 rA = proxyA->m_radius; float32 rB = proxyB->m_radius; if (output->distance > rA + rB && output->distance > b2_epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output->distance -= rA + rB; b2Vec2 normal = output->pointB - output->pointA; normal.Normalize(); output->pointA += rA * normal; output->pointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. b2Vec2 p = 0.5f * (output->pointA + output->pointB); output->pointA = p; output->pointB = p; output->distance = 0.0f; } } }
float32 b2TimeOfImpact(const b2TOIInput* input, const TA* shapeA, const TB* shapeB) { b2Sweep sweepA = input->sweepA; b2Sweep sweepB = input->sweepB; float32 r1 = input->sweepRadiusA; float32 r2 = input->sweepRadiusB; float32 tolerance = input->tolerance; float32 radius = shapeA->m_radius + shapeB->m_radius; b2Assert(sweepA.t0 == sweepB.t0); b2Assert(1.0f - sweepA.t0 > B2_FLT_EPSILON); b2Vec2 v1 = sweepA.c - sweepA.c0; b2Vec2 v2 = sweepB.c - sweepB.c0; float32 omega1 = sweepA.a - sweepA.a0; float32 omega2 = sweepB.a - sweepB.a0; float32 alpha = 0.0f; b2DistanceInput distanceInput; distanceInput.useRadii = false; b2SimplexCache cache; cache.count = 0; b2Vec2 p1, p2; const int32 k_maxIterations = 1000; // TODO_ERIN b2Settings int32 iter = 0; b2Vec2 normal = b2Vec2_zero; float32 distance = 0.0f; float32 targetDistance = 0.0f; for(;;) { b2XForm xf1, xf2; sweepA.GetTransform(&xf1, alpha); sweepB.GetTransform(&xf2, alpha); // Get the distance between shapes. distanceInput.transformA = xf1; distanceInput.transformB = xf2; b2DistanceOutput distanceOutput; b2Distance(&distanceOutput, &cache, &distanceInput, shapeA, shapeB); distance = distanceOutput.distance; p1 = distanceOutput.pointA; p2 = distanceOutput.pointB; if (iter == 0) { // Compute a reasonable target distance to give some breathing room // for conservative advancement. if (distance > radius) { targetDistance = b2Max(radius - tolerance, 0.75f * radius); } else { targetDistance = b2Max(distance - tolerance, 0.02f * radius); } } if (distance - targetDistance < 0.5f * tolerance || iter == k_maxIterations) { break; } normal = p2 - p1; normal.Normalize(); // Compute upper bound on remaining movement. float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2; if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON) { alpha = 1.0f; break; } // Get the conservative time increment. Don't advance all the way. float32 dAlpha = (distance - targetDistance) / approachVelocityBound; //float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound; float32 newAlpha = alpha + dAlpha; // The shapes may be moving apart or a safe distance apart. if (newAlpha < 0.0f || 1.0f < newAlpha) { alpha = 1.0f; break; } // Ensure significant advancement. if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha) { break; } alpha = newAlpha; ++iter; } b2_maxToiIters = b2Max(iter, b2_maxToiIters); return alpha; }
void sKeyCallback(GLFWwindow*, int key, int scancode, int action, int mods) { if (action == GLFW_PRESS) { switch (key) { case GLFW_KEY_ESCAPE: // Quit glfwSetWindowShouldClose(mainWindow, GL_TRUE); break; case GLFW_KEY_LEFT: // Pan left if (mods == GLFW_MOD_CONTROL) { b2Vec2 newOrigin(2.0f, 0.0f); test->ShiftOrigin(newOrigin); } else { g_camera.m_center.x -= 0.5f; } break; case GLFW_KEY_RIGHT: // Pan right if (mods == GLFW_MOD_CONTROL) { b2Vec2 newOrigin(-2.0f, 0.0f); test->ShiftOrigin(newOrigin); } else { g_camera.m_center.x += 0.5f; } break; case GLFW_KEY_DOWN: // Pan down if (mods == GLFW_MOD_CONTROL) { b2Vec2 newOrigin(0.0f, 2.0f); test->ShiftOrigin(newOrigin); } else { g_camera.m_center.y -= 0.5f; } break; case GLFW_KEY_UP: // Pan up if (mods == GLFW_MOD_CONTROL) { b2Vec2 newOrigin(0.0f, -2.0f); test->ShiftOrigin(newOrigin); } else { g_camera.m_center.y += 0.5f; } break; case GLFW_KEY_HOME: // Reset view g_camera.m_zoom = 1.0f; g_camera.m_center.Set(0.0f, 20.0f); break; case GLFW_KEY_Z: // Zoom out g_camera.m_zoom = b2Min(1.1f * g_camera.m_zoom, 20.0f); break; case GLFW_KEY_X: // Zoom in g_camera.m_zoom = b2Max(0.9f * g_camera.m_zoom, 0.02f); break; case GLFW_KEY_R: // Reset test delete test; test = entry->createFcn(b2d_scripts[script_index].filepath); break; case GLFW_KEY_SPACE: // Launch a bomb. if (test) { test->LaunchBomb(); } break; case GLFW_KEY_P: // Pause settings.pause = !settings.pause; break; /* case GLFW_KEY_LEFT_BRACKET: // Switch to previous test --testSelection; if (testSelection < 0) { testSelection = testCount - 1; } break; case GLFW_KEY_RIGHT_BRACKET: // Switch to next test ++testSelection; if (testSelection == testCount) { testSelection = 0; } break; */ case GLFW_KEY_TAB: ui.showMenu = !ui.showMenu; default: if (test) { test->Keyboard(key); } } } else if (action == GLFW_RELEASE) { test->KeyboardUp(key); } // else GLFW_REPEAT }
static void Keyboard(unsigned char key, int x, int y) { B2_NOT_USED(x); B2_NOT_USED(y); switch (key) { case 27: #ifndef __APPLE__ // freeglut specific function glutLeaveMainLoop(); #endif 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; } #if ENABLE_GLUI if (glui) glui->sync_live(); #endif // ENABLE_GLUI break; // Press ] to next test. case ']': ++testSelection; if (testSelection == testCount) { testSelection = 0; } #if ENABLE_GLUI if (glui) glui->sync_live(); #endif // ENABLE_GLUI break; // Press ~ to enable / disable the fullscreen UI. case '~': fullscreenUI.SetEnabled(!fullscreenUI.GetEnabled()); break; // Press < to select the previous particle parameter setting. case '<': particleParameter.Decrement(); break; // Press > to select the next particle parameter setting. case '>': particleParameter.Increment(); break; default: if (test) { test->Keyboard(key); } } }
bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; // Compute fresh Jacobians b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = cB + rB - cA - rA; b2Vec2 axis = b2Mul(qA, m_localXAxisA); float32 a1 = b2Cross(d + rA, axis); float32 a2 = b2Cross(rB, axis); b2Vec2 perp = b2Mul(qA, m_localYAxisA); float32 s1 = b2Cross(d + rA, perp); float32 s2 = b2Cross(rB, perp); b2Vec3 impulse; b2Vec2 C1; C1.x = b2Dot(perp, d); C1.y = aB - aA - m_referenceAngle; float32 linearError = b2Abs(C1.x); float32 angularError = b2Abs(C1.y); bool active = false; float32 C2 = 0.0f; if (m_enableLimit) { float32 translation = b2Dot(axis, d); if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { // Prevent large angular corrections C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); linearError = b2Max(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 = b2Max(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 = b2Max(linearError, translation - m_upperTranslation); active = true; } } if (active) { float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float32 k12 = iA * s1 + iB * s2; float32 k13 = iA * s1 * a1 + iB * s2 * a2; float32 k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float32 k23 = iA * a1 + iB * a2; float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; b2Mat33 K; K.ex.Set(k11, k12, k13); K.ey.Set(k12, k22, k23); K.ez.Set(k13, k23, k33); b2Vec3 C; C.x = C1.x; C.y = C1.y; C.z = C2; impulse = K.Solve33(-C); } else { float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float32 k12 = iA * s1 + iB * s2; float32 k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } b2Mat22 K; K.ex.Set(k11, k12); K.ey.Set(k12, k22); b2Vec2 impulse1 = K.Solve(-C1); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } b2Vec2 P = impulse.x * perp + impulse.z * axis; float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return linearError <= b2_linearSlop && angularError <= b2_angularSlop; }
VIRTUAL void PlayerEnt::Update(const sf::Time& dt) { if (m_bRespawn) { m_bRespawn = false; DestroyChain(); m_pBody->SetTransform(m_spawnPos, 0); } for (int i = 0; i < EButton::EB_COUNT; ++i) { m_vButtons[i].Flush(dt.asSeconds()); } if (m_vButtons[EB_SHOOT].Pull()) { Shoot(); } if (m_vButtons[EB_DASH].Pull()) { Dash(); } if (m_fFlipTime > 0) { float desiredAngle = 0; m_fFlipTime -= dt.asSeconds(); if (m_fFlipTime <= 0) { float desiredAngle = 0; m_pBody->SetTransform(m_pBody->GetPosition(), 0); m_pBody->SetFixedRotation(true); } else { desiredAngle = 180 * DEG_RAD;// Lerp(0, 360, 1.0f - (m_fFlipTime / m_tValue.m_fFlipTime)); float bodyAngle = m_pBody->GetAngle(); float nextAngle = bodyAngle + m_pBody->GetAngularVelocity() / 60.0; float totalRotation = desiredAngle - nextAngle; while (totalRotation < -180 * DEG_RAD) totalRotation += 360 * DEG_RAD; while (totalRotation > 180 * DEG_RAD) totalRotation -= 360 * DEG_RAD; float desiredAngularVelocity = totalRotation * 60; float change = 10 * DEG_RAD; //allow 1 degree rotation per time step desiredAngularVelocity = Min(change, Max(-change, desiredAngularVelocity)); float impulse = m_pBody->GetInertia() * desiredAngularVelocity; m_pBody->SetFixedRotation(false); m_pBody->ApplyAngularImpulse(impulse, true); } } if (m_vButtons[EB_JUMP].Push() && m_bGrounded) { m_fJumpTime = m_tValue.m_fJumpTime; //m_pBody->ApplyLinearImpulse(b2Vec2(0, m_tValue.m_fJumpImpulse), m_pBody->GetLocalCenter(), true); m_sound.setBuffer(m_soundBufferJump); m_sound.play(); m_pDrawable->state->setAnimationByName(0, "jump", false); float jIpulse = m_pBody->GetMass() * m_tValue.m_fInitialJumpSpeed; m_pBody->ApplyLinearImpulse(b2Vec2(0, jIpulse), m_pBody->GetLocalCenter(), true); b2Vec2 jumpImpulse(0, -jIpulse * 0.5f); for (int i = 0; i < m_vGroundContacts.size(); ++i) { b2Fixture* pFixA = m_vGroundContacts[i]->GetFixtureA(); b2Fixture* pFixB = m_vGroundContacts[i]->GetFixtureB(); if (pFixA == m_pGroundSensor) { pFixB->GetBody()->ApplyLinearImpulse(jumpImpulse, pFixB->GetBody()->GetWorldCenter(), true); } else { pFixA->GetBody()->ApplyLinearImpulse(jumpImpulse, pFixA->GetBody()->GetWorldCenter(), true); } } } else if (m_vButtons[EB_JUMP].Pull()) { m_fJumpTime = -1.0f; } if (m_vButtons[EB_JUMP].Held() && m_fJumpTime > 0.0f) { m_fJumpTime -= dt.asSeconds(); float jIpulse = m_pBody->GetMass() * m_tValue.m_fTerminalJumpSpeed; m_pBody->ApplyLinearImpulse(b2Vec2(0, jIpulse), m_pBody->GetLocalCenter(), true); } m_pDrawable->update(dt.asSeconds()); m_pDrawable->setPosition(m_pBody->GetPosition().x, -m_pBody->GetPosition().y); // m_pDrawable->setRotation(m_pBody->GetAngle() * -RAD_DEG); b2Vec2 vel = m_pBody->GetLinearVelocity(); float desiredVel = 0; bool hasInput = false; if (m_vButtons[EB_RIGHT].Held()) { hasInput = true; if (m_bGrounded) desiredVel = b2Min(vel.x + m_tValue.m_fGroundAcceleration, m_tValue.m_fMaxSpeed); else desiredVel = b2Min(vel.x + m_tValue.m_fAirAcceleration, m_tValue.m_fMaxSpeed); } if (m_vButtons[EB_LEFT].Held()) { hasInput = true; if (m_bGrounded) desiredVel = b2Max(vel.x - m_tValue.m_fGroundAcceleration, -m_tValue.m_fMaxSpeed); else desiredVel = b2Max(vel.x - m_tValue.m_fAirAcceleration, -m_tValue.m_fMaxSpeed); } bool skip = false; if (!hasInput) //apply deceleration { if (m_bGrounded) desiredVel = vel.x * m_tValue.m_fGroundDeceleration; else desiredVel = vel.x * m_tValue.m_fAirDeceleration; } else if((m_vButtons[EB_LEFT].Held() && vel.x < -m_tValue.m_fMaxSpeed) || (m_vButtons[EB_RIGHT].Held() && vel.x > m_tValue.m_fMaxSpeed)) { skip = true; } if (!skip) { float velChange = desiredVel - vel.x; float impulse = m_pBody->GetMass() * velChange; //disregard time factor m_pBody->ApplyLinearImpulse(b2Vec2(impulse, 0), m_pBody->GetWorldCenter(), true); } if (m_vButtons[EB_RIGHT].Held()) m_pDrawable->skeleton->flipX = false; else if (m_vButtons[EB_LEFT].Held()) m_pDrawable->skeleton->flipX = true; //do animation state changes if (m_bGrounded) { if ((m_vButtons[EB_RIGHT].Push() && !m_vButtons[EB_LEFT].Held()) || (m_vButtons[EB_LEFT].Push() && !m_vButtons[EB_RIGHT].Held())) { //m_pStateData->setMixByName("idle", "run", 0.25); m_pDrawable->state->setAnimationByName(0, "run", true); } if ((m_vButtons[EB_RIGHT].Pull() && !m_vButtons[EB_LEFT].Held()) || (m_vButtons[EB_LEFT].Pull() && !m_vButtons[EB_RIGHT].Held())) { //m_pStateData->setMixByName("run", "idle", 0.25); m_pDrawable->state->setAnimationByName(0, "idle", true); } } if (m_pTongueContactUse) { //b2Vec2 vel = m_pTongueTip->GetLinearVelocity(); //b2Vec2 pnt = m_pTongueTip->GetPosition();// +vel.Normalize() * m_tValue.m_fTongueTipRadius; b2Vec2 pnt; b2WorldManifold mani; m_pTongueContactUse->GetWorldManifold(&mani); b2Body* otherBody; if (m_pTongueContactUse->GetFixtureA()->GetBody() == m_pTongueTip) { otherBody = m_pTongueContactUse->GetFixtureB()->GetBody(); pnt = otherBody->GetWorldPoint(m_TongueLocalCoords);// mani.points[0]; } else { otherBody = m_pTongueContactUse->GetFixtureA()->GetBody(); pnt = otherBody->GetWorldPoint(m_TongueLocalCoords);// mani.points[0]; } MakeRopeJoint(otherBody, pnt); m_pBody->GetWorld()->DestroyBody(m_pTongueTip); m_pTongueTip = null; m_pTongueContactUse = null; } if (m_pTongueTip && (m_pTongueTip->GetPosition() - m_pBody->GetPosition()).Length() > m_tValue.m_fTetherLength) { m_pTongueTip->GetWorld()->DestroyBody(m_pTongueTip); m_pTongueTip = null; } b2Vec2 dir = CalcShootDirection(); b2Vec2 pos = m_pBody->GetPosition() + b2Vec2(aimX, aimY) + b2Vec2(dir.x * aimLenX, dir.y * aimLenY); if (dir.x < 0) pos = m_pBody->GetPosition() + b2Vec2(-aimX, aimY) + b2Vec2(dir.x * aimLenX, dir.y * aimLenY); if (m_pTongueTip) { pos = m_pTongueTip->GetPosition(); pos.x -= m_pCircleDraw->getRadius(); pos.y += m_pCircleDraw->getRadius(); } pos.y *= -1; m_pCircleDraw->setPosition(pos.x, pos.y); }
void b2VoronoiDiagram::Generate(float32 radius, float32 margin) { b2Assert(m_diagram == NULL); float32 inverseRadius = 1 / radius; b2Vec2 lower(+b2_maxFloat, +b2_maxFloat); b2Vec2 upper(-b2_maxFloat, -b2_maxFloat); for (int32 k = 0; k < m_generatorCount; k++) { Generator& g = m_generatorBuffer[k]; if (g.necessary) { lower = b2Min(lower, g.center); upper = b2Max(upper, g.center); } } lower.x -= margin; lower.y -= margin; upper.x += margin; upper.y += margin; m_countX = 1 + (int32) (inverseRadius * (upper.x - lower.x)); m_countY = 1 + (int32) (inverseRadius * (upper.y - lower.y)); m_diagram = (Generator**) m_allocator->Allocate(sizeof(Generator*) * m_countX * m_countY); for (int32 i = 0; i < m_countX * m_countY; i++) { m_diagram[i] = NULL; } // (4 * m_countX * m_countY) is the queue capacity that is experimentally // known to be necessary and sufficient for general particle distributions. b2StackQueue<b2VoronoiDiagramTask> queue( m_allocator, 4 * m_countX * m_countY); for (int32 k = 0; k < m_generatorCount; k++) { Generator& g = m_generatorBuffer[k]; g.center = inverseRadius * (g.center - lower); int32 x = (int32) g.center.x; int32 y = (int32) g.center.y; if (x >=0 && y >= 0 && x < m_countX && y < m_countY) { queue.Push(b2VoronoiDiagramTask(x, y, x + y * m_countX, &g)); } } while (!queue.Empty()) { int32 x = queue.Front().m_x; int32 y = queue.Front().m_y; int32 i = queue.Front().m_i; Generator* g = queue.Front().m_generator; queue.Pop(); if (!m_diagram[i]) { m_diagram[i] = g; if (x > 0) { queue.Push(b2VoronoiDiagramTask(x - 1, y, i - 1, g)); } if (y > 0) { queue.Push(b2VoronoiDiagramTask(x, y - 1, i - m_countX, g)); } if (x < m_countX - 1) { queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, g)); } if (y < m_countY - 1) { queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, g)); } } } for (int32 y = 0; y < m_countY; y++) { for (int32 x = 0; x < m_countX - 1; x++) { int32 i = x + y * m_countX; Generator* a = m_diagram[i]; Generator* b = m_diagram[i + 1]; if (a != b) { queue.Push(b2VoronoiDiagramTask(x, y, i, b)); queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, a)); } } } for (int32 y = 0; y < m_countY - 1; y++) { for (int32 x = 0; x < m_countX; x++) { int32 i = x + y * m_countX; Generator* a = m_diagram[i]; Generator* b = m_diagram[i + m_countX]; if (a != b) { queue.Push(b2VoronoiDiagramTask(x, y, i, b)); queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, a)); } } } while (!queue.Empty()) { const b2VoronoiDiagramTask& task = queue.Front(); int32 x = task.m_x; int32 y = task.m_y; int32 i = task.m_i; Generator* k = task.m_generator; queue.Pop(); Generator* a = m_diagram[i]; Generator* b = k; if (a != b) { float32 ax = a->center.x - x; float32 ay = a->center.y - y; float32 bx = b->center.x - x; float32 by = b->center.y - y; float32 a2 = ax * ax + ay * ay; float32 b2 = bx * bx + by * by; if (a2 > b2) { m_diagram[i] = b; if (x > 0) { queue.Push(b2VoronoiDiagramTask(x - 1, y, i - 1, b)); } if (y > 0) { queue.Push(b2VoronoiDiagramTask(x, y - 1, i - m_countX, b)); } if (x < m_countX - 1) { queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, b)); } if (y < m_countY - 1) { queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, b)); } } } } }
bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte) { float32 minSeparation = 0.0f; for (int32 i = 0; i < m_constraintCount; ++i) { b2ContactConstraint* c = m_constraints + i; b2Body* b1 = c->body1; b2Body* b2 = c->body2; float32 invMass1 = b1->m_mass * b1->m_invMass; float32 invI1 = b1->m_mass * b1->m_invI; float32 invMass2 = b2->m_mass * b2->m_invMass; float32 invI2 = b2->m_mass * b2->m_invI; b2Vec2 normal = c->normal; // Solver normal constraints for (int32 j = 0; j < c->pointCount; ++j) { b2ContactConstraintPoint* ccp = c->points + j; b2Vec2 r1 = b2Mul(b1->m_xf.R, ccp->localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->m_xf.R, ccp->localAnchor2 - b2->GetLocalCenter()); b2Vec2 p1 = b1->m_sweep.c + r1; b2Vec2 p2 = b2->m_sweep.c + r2; b2Vec2 dp = p2 - p1; // Approximate the current separation. float32 separation = b2Dot(dp, normal) + ccp->separation; // Track max constraint error. minSeparation = b2Min(minSeparation, separation); // Prevent large corrections and allow slop. float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); // Compute normal impulse float32 dImpulse = -ccp->equalizedMass * C; // b2Clamp the accumulated impulse float32 impulse0 = ccp->positionImpulse; ccp->positionImpulse = b2Max(impulse0 + dImpulse, 0.0f); dImpulse = ccp->positionImpulse - impulse0; b2Vec2 impulse = dImpulse * normal; b1->m_sweep.c -= invMass1 * impulse; b1->m_sweep.a -= invI1 * b2Cross(r1, impulse); b1->SynchronizeTransform(); b2->m_sweep.c += invMass2 * impulse; b2->m_sweep.a += invI2 * b2Cross(r2, impulse); b2->SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return minSeparation >= -1.5f * b2_linearSlop; }
void b2ContactSolver::SolveVelocityConstraints() { for (int32 i = 0; i < m_constraintCount; ++i) { b2ContactConstraint* c = m_constraints + i; b2Body* b1 = c->body1; b2Body* b2 = c->body2; float32 invMass1 = b1->m_invMass; float32 invI1 = b1->m_invI; float32 invMass2 = b2->m_invMass; float32 invI2 = b2->m_invI; b2Vec2 normal = c->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); // Solve normal constraints for (int32 j = 0; j < c->pointCount; ++j) { b2ContactConstraintPoint* ccp = c->points + j; b2Vec2 r1 = b2Mul(b1->m_xf.R, ccp->localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->m_xf.R, ccp->localAnchor2 - b2->GetLocalCenter()); // Relative velocity at contact b2Vec2 dv = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1); // Compute normal force float32 vn = b2Dot(dv, normal); float32 lambda = - m_step.inv_dt * ccp->normalMass * (vn - ccp->velocityBias); // b2Clamp the accumulated force float32 newForce = b2Max(ccp->normalForce + lambda, 0.0f); lambda = newForce - ccp->normalForce; // Apply contact impulse b2Vec2 P = m_step.dt * lambda * normal; b1->m_linearVelocity -= invMass1 * P; b1->m_angularVelocity -= invI1 * b2Cross(r1, P); b2->m_linearVelocity += invMass2 * P; b2->m_angularVelocity += invI2 * b2Cross(r2, P); ccp->normalForce = newForce; } // Solve tangent constraints for (int32 j = 0; j < c->pointCount; ++j) { b2ContactConstraintPoint* ccp = c->points + j; b2Vec2 r1 = b2Mul(b1->m_xf.R, ccp->localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->m_xf.R, ccp->localAnchor2 - b2->GetLocalCenter()); // Relative velocity at contact b2Vec2 dv = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1); // Compute tangent force float32 vt = b2Dot(dv, tangent); float32 lambda = m_step.inv_dt * ccp->tangentMass * (-vt); // b2Clamp the accumulated force float32 maxFriction = c->friction * ccp->normalForce; float32 newForce = b2Clamp(ccp->tangentForce + lambda, -maxFriction, maxFriction); lambda = newForce - ccp->tangentForce; // Apply contact impulse b2Vec2 P = m_step.dt * lambda * tangent; b1->m_linearVelocity -= invMass1 * P; b1->m_angularVelocity -= invI1 * b2Cross(r1, P); b2->m_linearVelocity += invMass2 * P; b2->m_angularVelocity += invI2 * b2Cross(r2, P); ccp->tangentForce = newForce; } } }
void Test::Step(Settings* settings) { float32 timeStep = settings->hz > 0.0f ? 1.0f / settings->hz : float32(0.0f); if (settings->pause) { if (settings->singleStep) { settings->singleStep = 0; } else { timeStep = 0.0f; } m_debugDraw.DrawString(5, m_textLine, "****PAUSED****"); m_textLine += 15; } uint32 flags = 0; flags += settings->drawShapes * b2Draw::e_shapeBit; flags += settings->drawJoints * b2Draw::e_jointBit; flags += settings->drawAABBs * b2Draw::e_aabbBit; flags += settings->drawPairs * b2Draw::e_pairBit; flags += settings->drawCOMs * b2Draw::e_centerOfMassBit; m_debugDraw.SetFlags(flags); m_world->SetWarmStarting(settings->enableWarmStarting > 0); m_world->SetContinuousPhysics(settings->enableContinuous > 0); m_world->SetSubStepping(settings->enableSubStepping > 0); m_pointCount = 0; m_world->Step(timeStep, settings->velocityIterations, settings->positionIterations); m_world->DrawDebugData(); if (timeStep > 0.0f) { ++m_stepCount; } if (settings->drawStats) { int32 bodyCount = m_world->GetBodyCount(); int32 contactCount = m_world->GetContactCount(); int32 jointCount = m_world->GetJointCount(); m_debugDraw.DrawString(5, m_textLine, "bodies/contacts/joints = %d/%d/%d", bodyCount, contactCount, jointCount); m_textLine += 15; int32 proxyCount = m_world->GetProxyCount(); int32 height = m_world->GetTreeHeight(); int32 balance = m_world->GetTreeBalance(); float32 quality = m_world->GetTreeQuality(); m_debugDraw.DrawString(5, m_textLine, "proxies/height/balance/quality = %d/%d/%d/%g", proxyCount, height, balance, quality); m_textLine += 15; } // Track maximum profile times { const b2Profile& p = m_world->GetProfile(); m_maxProfile.step = b2Max(m_maxProfile.step, p.step); m_maxProfile.collide = b2Max(m_maxProfile.collide, p.collide); m_maxProfile.solve = b2Max(m_maxProfile.solve, p.solve); m_maxProfile.solveInit = b2Max(m_maxProfile.solveInit, p.solveInit); m_maxProfile.solveVelocity = b2Max(m_maxProfile.solveVelocity, p.solveVelocity); m_maxProfile.solvePosition = b2Max(m_maxProfile.solvePosition, p.solvePosition); m_maxProfile.solveTOI = b2Max(m_maxProfile.solveTOI, p.solveTOI); m_maxProfile.broadphase = b2Max(m_maxProfile.broadphase, p.broadphase); m_totalProfile.step += p.step; m_totalProfile.collide += p.collide; m_totalProfile.solve += p.solve; m_totalProfile.solveInit += p.solveInit; m_totalProfile.solveVelocity += p.solveVelocity; m_totalProfile.solvePosition += p.solvePosition; m_totalProfile.solveTOI += p.solveTOI; m_totalProfile.broadphase += p.broadphase; } if (settings->drawProfile) { const b2Profile& p = m_world->GetProfile(); b2Profile aveProfile; memset(&aveProfile, 0, sizeof(b2Profile)); if (m_stepCount > 0) { float32 scale = 1.0f / m_stepCount; aveProfile.step = scale * m_totalProfile.step; aveProfile.collide = scale * m_totalProfile.collide; aveProfile.solve = scale * m_totalProfile.solve; aveProfile.solveInit = scale * m_totalProfile.solveInit; aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity; aveProfile.solvePosition = scale * m_totalProfile.solvePosition; aveProfile.solveTOI = scale * m_totalProfile.solveTOI; aveProfile.broadphase = scale * m_totalProfile.broadphase; } m_debugDraw.DrawString(5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase); m_textLine += 15; } if (m_mouseJoint) { b2Vec2 p1 = m_mouseJoint->GetAnchorB(); b2Vec2 p2 = m_mouseJoint->GetTarget(); b2Color c; c.Set(0.0f, 1.0f, 0.0f); m_debugDraw.DrawPoint(p1, 4.0f, c); m_debugDraw.DrawPoint(p2, 4.0f, c); c.Set(0.8f, 0.8f, 0.8f); m_debugDraw.DrawSegment(p1, p2, c); } if (m_bombSpawning) { b2Color c; c.Set(0.0f, 0.0f, 1.0f); m_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c); c.Set(0.8f, 0.8f, 0.8f); m_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c); } if (settings->drawContactPoints) { //const float32 k_impulseScale = 0.1f; const float32 k_axisScale = 0.3f; for (int32 i = 0; i < m_pointCount; ++i) { ContactPoint* point = m_points + i; if (point->state == b2_addState) { // Add m_debugDraw.DrawPoint(point->position, 10.0f, b2Color(0.3f, 0.95f, 0.3f)); } else if (point->state == b2_persistState) { // Persist m_debugDraw.DrawPoint(point->position, 5.0f, b2Color(0.3f, 0.3f, 0.95f)); } if (settings->drawContactNormals == 1) { b2Vec2 p1 = point->position; b2Vec2 p2 = p1 + k_axisScale * point->normal; m_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.9f)); } else if (settings->drawContactForces == 1) { //b2Vec2 p1 = point->position; //b2Vec2 p2 = p1 + k_forceScale * point->normalForce * point->normal; //DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f)); } if (settings->drawFrictionForces == 1) { //b2Vec2 tangent = b2Cross(point->normal, 1.0f); //b2Vec2 p1 = point->position; //b2Vec2 p2 = p1 + k_forceScale * point->tangentForce * tangent; //DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f)); } } } }
// Find islands, integrate and solve constraints, solve position constraints void b2World::Solve(const b2TimeStep& step) { m_positionIterationCount = 0; // Size the island for the worst case. b2Island island(m_bodyCount, m_contactCount, m_jointCount, &m_stackAllocator, m_contactListener); // Clear all the island flags. for (b2Body* b = m_bodyList; b; b = b->m_next) { b->m_flags &= ~b2Body::e_islandFlag; } for (b2Contact* c = m_contactList; c; c = c->m_next) { c->m_flags &= ~b2Contact::e_islandFlag; } for (b2Joint* j = m_jointList; j; j = j->m_next) { j->m_islandFlag = false; } // Build and simulate all awake islands. int32 stackSize = m_bodyCount; b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) { if (seed->m_flags & (b2Body::e_islandFlag | b2Body::e_sleepFlag | b2Body::e_frozenFlag)) { continue; } if (seed->IsStatic()) { continue; } // Reset island and stack. island.Clear(); int32 stackCount = 0; stack[stackCount++] = seed; seed->m_flags |= b2Body::e_islandFlag; // Perform a depth first search (DFS) on the constraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. b2Body* b = stack[--stackCount]; island.Add(b); // Make sure the body is awake. b->m_flags &= ~b2Body::e_sleepFlag; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b->IsStatic()) { continue; } // Search all contacts connected to this body. for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next) { // Has this contact already been added to an island? if (cn->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_nonSolidFlag)) { continue; } // Is this contact touching? if (cn->contact->GetManifoldCount() == 0) { continue; } island.Add(cn->contact); cn->contact->m_flags |= b2Contact::e_islandFlag; b2Body* other = cn->other; // Was the other body already added to this island? if (other->m_flags & b2Body::e_islandFlag) { continue; } b2Assert(stackCount < stackSize); stack[stackCount++] = other; other->m_flags |= b2Body::e_islandFlag; } // Search all joints connect to this body. for (b2JointEdge* jn = b->m_jointList; jn; jn = jn->next) { if (jn->joint->m_islandFlag == true) { continue; } island.Add(jn->joint); jn->joint->m_islandFlag = true; b2Body* other = jn->other; if (other->m_flags & b2Body::e_islandFlag) { continue; } b2Assert(stackCount < stackSize); stack[stackCount++] = other; other->m_flags |= b2Body::e_islandFlag; } } island.Solve(step, m_gravity, s_enablePositionCorrection > 0, m_allowSleep); m_positionIterationCount = b2Max(m_positionIterationCount, island.m_positionIterationCount); // Post solve cleanup. for (int32 i = 0; i < island.m_bodyCount; ++i) { // Allow static bodies to participate in other islands. b2Body* b = island.m_bodies[i]; if (b->IsStatic()) { b->m_flags &= ~b2Body::e_islandFlag; } } } m_stackAllocator.Free(stack); // Synchronize shapes, check for out of range bodies. for (b2Body* b = m_bodyList; b; b = b->GetNext()) { if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag)) { continue; } if (b->IsStatic()) { continue; } // Update shapes (for broad-phase). If the shapes go out of // the world AABB then shapes and contacts may be destroyed, // including contacts that are bool inRange = b->SynchronizeShapes(); // Did the body's shapes leave the world? if (inRange == false && m_boundaryListener != NULL) { m_boundaryListener->Violation(b); } } // Commit shape proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_broadPhase->Commit(); }
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) { ++b2_toiCalls; output->state = b2TOIOutput::e_unknown; output->t = input->tMax; const b2DistanceProxy* proxyA = &input->proxyA; const b2DistanceProxy* proxyB = &input->proxyB; b2Sweep sweepA = input->sweepA; b2Sweep sweepB = input->sweepB; // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float32 tMax = input->tMax; float32 totalRadius = proxyA->m_radius + proxyB->m_radius; float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); float32 tolerance = 0.25f * b2_linearSlop; b2Assert(target > tolerance); float32 t1 = 0.0f; const int32 k_maxIterations = 20; // TODO_ERIN b2Settings int32 iter = 0; // Prepare input for distance query. b2SimplexCache cache; cache.count = 0; b2DistanceInput distanceInput; distanceInput.proxyA = input->proxyA; distanceInput.proxyB = input->proxyB; distanceInput.useRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for(;;) { b2Transform xfA, xfB; sweepA.GetTransform(&xfA, t1); sweepB.GetTransform(&xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. distanceInput.transformA = xfA; distanceInput.transformB = xfB; b2DistanceOutput distanceOutput; b2Distance(&distanceOutput, &cache, &distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output->state = b2TOIOutput::e_overlapped; output->t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output->state = b2TOIOutput::e_touching; output->t = t1; break; } // Initialize the separating axis. b2SeparationFunction fcn; fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB); #if 0 // Dump the curve seen by the root finder { const int32 N = 100; float32 dx = 1.0f / N; float32 xs[N+1]; float32 fs[N+1]; float32 x = 0.0f; for (int32 i = 0; i <= N; ++i) { sweepA.GetTransform(&xfA, x); sweepB.GetTransform(&xfB, x); float32 f = fcn.Evaluate(xfA, xfB) - target; printf("%g %g\n", x, f); xs[i] = x; fs[i] = f; x += dx; } } #endif // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of vertices. bool done = false; float32 t2 = tMax; int32 pushBackIter = 0; for (;;) { // Find the deepest point at t2. Store the witness point indices. int32 indexA, indexB; float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output->state = b2TOIOutput::e_separated; output->t = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float32 s1 = fcn.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output->state = b2TOIOutput::e_failed; output->t = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // Victory! t1 should hold the TOI (could be 0.0). output->state = b2TOIOutput::e_touching; output->t = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int32 rootIterCount = 0; float32 a1 = t1, a2 = t2; for (;;) { // Use a mix of the secant rule and bisection. float32 t; if (rootIterCount & 1) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } float32 s = fcn.Evaluate(indexA, indexB, t); if (b2Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } ++rootIterCount; ++b2_toiRootIters; if (rootIterCount == 50) { break; } } b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == b2_maxPolygonVertices) { break; } } ++iter; ++b2_toiIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output->state = b2TOIOutput::e_failed; output->t = t1; break; } } b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); }
void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; // Solve linear motor constraint. if (m_enableMotor && m_limitState != e_equalLimits) { float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; float32 impulse = m_motorMass * (m_motorSpeed - Cdot); float32 oldImpulse = m_motorImpulse; float32 maxImpulse = data.step.dt * m_maxMotorForce; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; b2Vec2 P = impulse * m_axis; float32 LA = impulse * m_a1; float32 LB = impulse * m_a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } b2Vec2 Cdot1; Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; if (m_enableLimit && m_limitState != e_inactiveLimit) { // Solve prismatic and limit constraint in block form. float32 Cdot2; Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 f1 = m_impulse; b2Vec3 df = m_K.Solve33(-Cdot); m_impulse += df; if (m_limitState == e_atLowerLimit) { m_impulse.z = b2Max(m_impulse.z, 0.0f); } else if (m_limitState == e_atUpperLimit) { m_impulse.z = b2Min(m_impulse.z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); m_impulse.x = f2r.x; m_impulse.y = f2r.y; df = m_impulse - f1; b2Vec2 P = df.x * m_perp + df.z * m_axis; float32 LA = df.x * m_s1 + df.y + df.z * m_a1; float32 LB = df.x * m_s2 + df.y + df.z * m_a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { // Limit is inactive, just solve the prismatic constraint in block form. b2Vec2 df = m_K.Solve22(-Cdot1); m_impulse.x += df.x; m_impulse.y += df.y; b2Vec2 P = df.x * m_perp; float32 LA = df.x * m_s1 + df.y; float32 LB = df.x * m_s2 + df.y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; b2Vec2 Cdot10 = Cdot1; Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; Cdot1.y = wB - wA; if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f) { b2Vec2 test = b2Mul22(m_K, df); Cdot1.x += 0.0f; } } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2World::Step(float32 dt, int32 iterations) { b2TimeStep step; step.dt = dt; step.iterations = iterations; if (dt > 0.0f) { step.inv_dt = 1.0f / dt; } else { step.inv_dt = 0.0f; } m_positionIterationCount = 0; // Handle deferred contact destruction. m_contactManager.CleanContactList(); // Handle deferred body destruction. CleanBodyList(); // Update contacts. m_contactManager.Collide(); // Size the island for the worst case. b2Island island(m_bodyCount, m_contactCount, m_jointCount, &m_stackAllocator); // Clear all the island flags. for (b2Body* b = m_bodyList; b; b = b->m_next) { b->m_flags &= ~b2Body::e_islandFlag; } for (b2Contact* c = m_contactList; c; c = c->m_next) { c->m_flags &= ~b2Contact::e_islandFlag; } for (b2Joint* j = m_jointList; j; j = j->m_next) { j->m_islandFlag = false; } // Build and simulate all awake islands. int32 stackSize = m_bodyCount; b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) { if (seed->m_flags & (b2Body::e_staticFlag | b2Body::e_islandFlag | b2Body::e_sleepFlag | b2Body::e_frozenFlag)) { continue; } // Reset island and stack. island.Clear(); int32 stackCount = 0; stack[stackCount++] = seed; seed->m_flags |= b2Body::e_islandFlag; // Perform a depth first search (DFS) on the constraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. b2Body* b = stack[--stackCount]; island.Add(b); // Make sure the body is awake. b->m_flags &= ~b2Body::e_sleepFlag; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b->m_flags & b2Body::e_staticFlag) { continue; } // Search all contacts connected to this body. for (b2ContactNode* cn = b->m_contactList; cn; cn = cn->next) { if (cn->contact->m_flags & b2Contact::e_islandFlag) { continue; } island.Add(cn->contact); cn->contact->m_flags |= b2Contact::e_islandFlag; b2Body* other = cn->other; if (other->m_flags & b2Body::e_islandFlag) { continue; } b2Assert(stackCount < stackSize); stack[stackCount++] = other; other->m_flags |= b2Body::e_islandFlag; } // Search all joints connect to this body. for (b2JointNode* jn = b->m_jointList; jn; jn = jn->next) { if (jn->joint->m_islandFlag == true) { continue; } island.Add(jn->joint); jn->joint->m_islandFlag = true; b2Body* other = jn->other; if (other->m_flags & b2Body::e_islandFlag) { continue; } b2Assert(stackCount < stackSize); stack[stackCount++] = other; other->m_flags |= b2Body::e_islandFlag; } } island.Solve(&step, m_gravity); m_positionIterationCount = b2Max(m_positionIterationCount, island.m_positionIterationCount); if (m_allowSleep) { island.UpdateSleep(dt); } // Post solve cleanup. for (int32 i = 0; i < island.m_bodyCount; ++i) { // Allow static bodies to participate in other islands. b2Body* b = island.m_bodies[i]; if (b->m_flags & b2Body::e_staticFlag) { b->m_flags &= ~b2Body::e_islandFlag; } // Handle newly frozen bodies. if (b->IsFrozen() && m_listener) { b2BoundaryResponse response = m_listener->NotifyBoundaryViolated(b); if (response == b2_destroyBody) { DestroyBody(b); b = NULL; island.m_bodies[i] = NULL; } } } } m_stackAllocator.Free(stack); m_broadPhase->Commit(); #if 0 for (b2Contact* c = m_contactList; c; c = c->GetNext()) { b2Shape* shape1 = c->GetShape1(); b2Shape* shape2 = c->GetShape2(); b2Body* body1 = shape1->GetBody(); b2Body* body2 = shape2->GetBody(); if (body1->IsSleeping() && body2->IsSleeping()) { continue; } b2Conservative(shape1, shape2); } #endif }
void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step) { b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; b2Vec2 v1 = b1->m_linearVelocity; float32 w1 = b1->m_angularVelocity; b2Vec2 v2 = b2->m_linearVelocity; float32 w2 = b2->m_angularVelocity; // Solve linear motor constraint. if (m_enableMotor && m_limitState != e_equalLimits) { float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; float32 impulse = m_motorMass * (m_motorSpeed - Cdot); float32 oldImpulse = m_motorImpulse; float32 maxImpulse = step.dt * m_maxMotorForce; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; b2Vec2 P = impulse * m_axis; float32 L1 = impulse * m_a1; float32 L2 = impulse * m_a2; v1 -= m_invMassA * P; w1 -= m_invIA * L1; v2 += m_invMassB * P; w2 += m_invIB * L2; } b2Vec2 Cdot1; Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1; Cdot1.y = w2 - w1; if (m_enableLimit && m_limitState != e_inactiveLimit) { // Solve prismatic and limit constraint in block form. float32 Cdot2; Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 f1 = m_impulse; b2Vec3 df = m_K.Solve33(-Cdot); m_impulse += df; if (m_limitState == e_atLowerLimit) { m_impulse.z = b2Max(m_impulse.z, 0.0f); } else if (m_limitState == e_atUpperLimit) { m_impulse.z = b2Min(m_impulse.z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y); b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); m_impulse.x = f2r.x; m_impulse.y = f2r.y; df = m_impulse - f1; b2Vec2 P = df.x * m_perp + df.z * m_axis; float32 L1 = df.x * m_s1 + df.y + df.z * m_a1; float32 L2 = df.x * m_s2 + df.y + df.z * m_a2; v1 -= m_invMassA * P; w1 -= m_invIA * L1; v2 += m_invMassB * P; w2 += m_invIB * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. b2Vec2 df = m_K.Solve22(-Cdot1); m_impulse.x += df.x; m_impulse.y += df.y; b2Vec2 P = df.x * m_perp; float32 L1 = df.x * m_s1 + df.y; float32 L2 = df.x * m_s2 + df.y; v1 -= m_invMassA * P; w1 -= m_invIA * L1; v2 += m_invMassB * P; w2 += m_invIB * L2; } b1->m_linearVelocity = v1; b1->m_angularVelocity = w1; b2->m_linearVelocity = v2; b2->m_angularVelocity = w2; }
void callbacks_t::keyboard_cb(unsigned char key, int x, int y) { //! What are these? B2_NOT_USED(x); B2_NOT_USED(y); switch (key) { case 27: exit(0); break; //! Press 'z' to zoom out. case 'z': view_zoom = b2Min(1.1f * view_zoom, 20.0f); resize_cb(width, height); break; //! Press 'x' to zoom in. case 'x': //(sim->create_fcn())->launch(); view_zoom = b2Max(0.9f * view_zoom, 0.02f); resize_cb(width, height); break; //! Press 'r' to reset. case 'r': delete test; test = entry->create_fcn(); break; //! Press 'p' to pause. case 'p': settings.pause = !settings.pause; break; case 'w': (test)->launch(); break; /*case 's': (test)->delaunch(); break;*/ case '8': (test)->launch2(); break; /*case '2': (test)->delaunch2(); break;*/ /*case 'n': (test)->ball(); break;*/ /*case 'm': (test)->createball(); break;*/ case 'a': (test)->flipperleft(); break; case 'd': (test)->flipperright(); //(test)->flipperwheelright(); break; case '4': (test)->flipperwheelleft(); break; case '6': (test)->flipperwheelright(); break; //! The default case. Why is this needed? default: if (test) { test->keyboard(key); } } }
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, , 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; #if B2_DEBUG_SOLVER == 1 vn1 = 0.0f; vn2 = vc->K.ex.y * x.x + b.y; #endif 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; #if B2_DEBUG_SOLVER == 1 vn1 = vc->K.ey.x * x.y + b.x; vn2 = 0.0f; #endif 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 b2VoronoiDiagram::Generate(float32 radius) { b2Assert(m_diagram == NULL); float32 inverseRadius = 1 / radius; b2Vec2 lower(+b2_maxFloat, +b2_maxFloat); b2Vec2 upper(-b2_maxFloat, -b2_maxFloat); for (int32 k = 0; k < m_generatorCount; k++) { Generator& g = m_generatorBuffer[k]; lower = b2Min(lower, g.center); upper = b2Max(upper, g.center); } m_countX = 1 + (int32) (inverseRadius * (upper.x - lower.x)); m_countY = 1 + (int32) (inverseRadius * (upper.y - lower.y)); m_diagram = (Generator**) m_allocator->Allocate(sizeof(Generator*) * m_countX * m_countY); for (int32 i = 0; i < m_countX * m_countY; i++) { m_diagram[i] = NULL; } b2StackQueue<b2VoronoiDiagramTask> queue( m_allocator, 4 * m_countX * m_countX); for (int32 k = 0; k < m_generatorCount; k++) { Generator& g = m_generatorBuffer[k]; g.center = inverseRadius * (g.center - lower); int32 x = b2Max(0, b2Min((int32) g.center.x, m_countX - 1)); int32 y = b2Max(0, b2Min((int32) g.center.y, m_countY - 1)); queue.Push(b2VoronoiDiagramTask(x, y, x + y * m_countX, &g)); } while (!queue.Empty()) { int32 x = queue.Front().m_x; int32 y = queue.Front().m_y; int32 i = queue.Front().m_i; Generator* g = queue.Front().m_generator; queue.Pop(); if (!m_diagram[i]) { m_diagram[i] = g; if (x > 0) { queue.Push(b2VoronoiDiagramTask(x - 1, y, i - 1, g)); } if (y > 0) { queue.Push(b2VoronoiDiagramTask(x, y - 1, i - m_countX, g)); } if (x < m_countX - 1) { queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, g)); } if (y < m_countY - 1) { queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, g)); } } } int32 maxIteration = m_countX + m_countY; for (int32 iteration = 0; iteration < maxIteration; iteration++) { for (int32 y = 0; y < m_countY; y++) { for (int32 x = 0; x < m_countX - 1; x++) { int32 i = x + y * m_countX; Generator* a = m_diagram[i]; Generator* b = m_diagram[i + 1]; if (a != b) { queue.Push(b2VoronoiDiagramTask(x, y, i, b)); queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, a)); } } } for (int32 y = 0; y < m_countY - 1; y++) { for (int32 x = 0; x < m_countX; x++) { int32 i = x + y * m_countX; Generator* a = m_diagram[i]; Generator* b = m_diagram[i + m_countX]; if (a != b) { queue.Push(b2VoronoiDiagramTask(x, y, i, b)); queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, a)); } } } bool updated = false; while (!queue.Empty()) { int32 x = queue.Front().m_x; int32 y = queue.Front().m_y; int32 i = queue.Front().m_i; Generator* k = queue.Front().m_generator; queue.Pop(); Generator* a = m_diagram[i]; Generator* b = k; if (a != b) { float32 ax = a->center.x - x; float32 ay = a->center.y - y; float32 bx = b->center.x - x; float32 by = b->center.y - y; float32 a2 = ax * ax + ay * ay; float32 b2 = bx * bx + by * by; if (a2 > b2) { m_diagram[i] = b; if (x > 0) { queue.Push(b2VoronoiDiagramTask(x - 1, y, i - 1, b)); } if (y > 0) { queue.Push(b2VoronoiDiagramTask(x, y - 1, i - m_countX, b)); } if (x < m_countX - 1) { queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, b)); } if (y < m_countY - 1) { queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, b)); } updated = true; } } } if (!updated) { break; } } }
float32 b2TimeOfImpact(const b2TOIInput* input, const TA* shapeA, const TB* shapeB) { b2Sweep sweepA = input->sweepA; b2Sweep sweepB = input->sweepB; b2Assert(sweepA.t0 == sweepB.t0); b2Assert(1.0f - sweepA.t0 > B2_FLT_EPSILON); float32 radius = shapeA->m_radius + shapeB->m_radius; float32 tolerance = input->tolerance; float32 alpha = 0.0f; const int32 k_maxIterations = 1000; // TODO_ERIN b2Settings int32 iter = 0; float32 target = 0.0f; // Prepare input for distance query. b2SimplexCache cache; cache.count = 0; b2DistanceInput distanceInput; distanceInput.useRadii = false; for(;;) { b2XForm xfA, xfB; sweepA.GetTransform(&xfA, alpha); sweepB.GetTransform(&xfB, alpha); // Get the distance between shapes. distanceInput.transformA = xfA; distanceInput.transformB = xfB; b2DistanceOutput distanceOutput; b2Distance(&distanceOutput, &cache, &distanceInput, shapeA, shapeB); if (distanceOutput.distance <= 0.0f) { alpha = 1.0f; break; } b2SeparationFunction<TA, TB> fcn; fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB); float32 separation = fcn.Evaluate(xfA, xfB); if (separation <= 0.0f) { alpha = 1.0f; break; } if (iter == 0) { // Compute a reasonable target distance to give some breathing room // for conservative advancement. We take advantage of the shape radii // to create additional clearance. if (separation > radius) { target = b2Max(radius - tolerance, 0.75f * radius); } else { target = b2Max(separation - tolerance, 0.02f * radius); } } if (separation - target < 0.5f * tolerance) { if (iter == 0) { alpha = 1.0f; break; } break; } #if 0 // Dump the curve seen by the root finder { const int32 N = 100; float32 dx = 1.0f / N; float32 xs[N+1]; float32 fs[N+1]; float32 x = 0.0f; for (int32 i = 0; i <= N; ++i) { sweepA.GetTransform(&xfA, x); sweepB.GetTransform(&xfB, x); float32 f = fcn.Evaluate(xfA, xfB) - target; printf("%g %g\n", x, f); xs[i] = x; fs[i] = f; x += dx; } } #endif // Compute 1D root of: f(x) - target = 0 float32 newAlpha = alpha; { float32 x1 = alpha, x2 = 1.0f; float32 f1 = separation; sweepA.GetTransform(&xfA, x2); sweepB.GetTransform(&xfB, x2); float32 f2 = fcn.Evaluate(xfA, xfB); // If intervals don't overlap at t2, then we are done. if (f2 >= target) { alpha = 1.0f; break; } // Determine when intervals intersect. int32 rootIterCount = 0; for (;;) { // Use a mix of the secant rule and bisection. float32 x; if (rootIterCount & 1) { // Secant rule to improve convergence. x = x1 + (target - f1) * (x2 - x1) / (f2 - f1); } else { // Bisection to guarantee progress. x = 0.5f * (x1 + x2); } sweepA.GetTransform(&xfA, x); sweepB.GetTransform(&xfB, x); float32 f = fcn.Evaluate(xfA, xfB); if (b2Abs(f - target) < 0.025f * tolerance) { newAlpha = x; break; } // Ensure we continue to bracket the root. if (f > target) { x1 = x; f1 = f; } else { x2 = x; f2 = f; } ++rootIterCount; //b2Assert(rootIterCount < 50); if (rootIterCount >= 50 ) { break; } } b2_maxToiRootIters = b2Max(b2_maxToiRootIters, rootIterCount); } // Ensure significant advancement. if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha) { break; } alpha = newAlpha; ++iter; if (iter == k_maxIterations) { break; } } b2_maxToiIters = b2Max(b2_maxToiIters, iter); return alpha; }
void b2DynamicTree::InsertLeaf(int32 leaf) { ++m_insertionCount; if (m_root == b2_nullNode) { m_root = leaf; m_nodes[m_root].parent = b2_nullNode; return; } // Find the best sibling for this node b2AABB leafAABB = m_nodes[leaf].aabb; int32 index = m_root; while (m_nodes[index].IsLeaf() == false) { int32 child1 = m_nodes[index].child1; int32 child2 = m_nodes[index].child2; float32 area = m_nodes[index].aabb.GetPerimeter(); b2AABB combinedAABB; combinedAABB.Combine(m_nodes[index].aabb, leafAABB); float32 combinedArea = combinedAABB.GetPerimeter(); // Cost of creating a new parent for this node and the new leaf float32 cost = 2.0f * combinedArea; // Minimum cost of pushing the leaf further down the tree float32 inheritanceCost = 2.0f * (combinedArea - area); // Cost of descending into child1 float32 cost1; if (m_nodes[child1].IsLeaf()) { b2AABB aabb; aabb.Combine(leafAABB, m_nodes[child1].aabb); cost1 = aabb.GetPerimeter() + inheritanceCost; } else { b2AABB aabb; aabb.Combine(leafAABB, m_nodes[child1].aabb); float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); float32 newArea = aabb.GetPerimeter(); cost1 = (newArea - oldArea) + inheritanceCost; } // Cost of descending into child2 float32 cost2; if (m_nodes[child2].IsLeaf()) { b2AABB aabb; aabb.Combine(leafAABB, m_nodes[child2].aabb); cost2 = aabb.GetPerimeter() + inheritanceCost; } else { b2AABB aabb; aabb.Combine(leafAABB, m_nodes[child2].aabb); float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); float32 newArea = aabb.GetPerimeter(); cost2 = newArea - oldArea + inheritanceCost; } // Descend according to the minimum cost. if (cost < cost1 && cost < cost2) { break; } // Descend if (cost1 < cost2) { index = child1; } else { index = child2; } } int32 sibling = index; // Create a new parent. int32 oldParent = m_nodes[sibling].parent; int32 newParent = AllocateNode(); m_nodes[newParent].parent = oldParent; m_nodes[newParent].userData = NULL; m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); m_nodes[newParent].height = m_nodes[sibling].height + 1; if (oldParent != b2_nullNode) { // The sibling was not the root. if (m_nodes[oldParent].child1 == sibling) { m_nodes[oldParent].child1 = newParent; } else { m_nodes[oldParent].child2 = newParent; } m_nodes[newParent].child1 = sibling; m_nodes[newParent].child2 = leaf; m_nodes[sibling].parent = newParent; m_nodes[leaf].parent = newParent; } else { // The sibling was the root. m_nodes[newParent].child1 = sibling; m_nodes[newParent].child2 = leaf; m_nodes[sibling].parent = newParent; m_nodes[leaf].parent = newParent; m_root = newParent; } // Walk back up the tree fixing heights and AABBs index = m_nodes[leaf].parent; while (index != b2_nullNode) { index = Balance(index); int32 child1 = m_nodes[index].child1; int32 child2 = m_nodes[index].child2; b2Assert(child1 != b2_nullNode); b2Assert(child2 != b2_nullNode); m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); index = m_nodes[index].parent; } //Validate(); }
bool Player::update(float deltaTime) { b2Vec2 velocity = _body->GetLinearVelocity(); b2Vec2 force(0.0f, 0.0f), acceleration(0.0f, 0.0f), desiredVelocity(0.0f, 0.0f); float maxSpeed = b2Max(_maxVelocity.x, _maxVelocity.y); //Determine animations if (_direction.x < 0) { _animationManager->play("BANK_LEFT"); } else if (_direction.x > 0) { _animationManager->play("BANK_RIGHT"); } else { if (_animationManager->getCurrentAnimationName() != "IDLE") { Ess2D::Animation* currentAnimation = _animationManager->getCurrent(); if (!currentAnimation->isReversed()) { currentAnimation->setReverse(true); } else if (currentAnimation->getCurrentFrameNumber() == 0) { _animationManager->play("IDLE"); } } } //Handle Physics if(_direction.x != 0) { acceleration.x = (_direction.x * _maxVelocity.x - velocity.x); } else if(velocity.x != 0) { acceleration.x = (_defaultVelocity.x - velocity.x); } if(_direction.y != 0) { if(_direction.y < 0) { acceleration.y = _direction.y * _maxVelocity.y - velocity.y; } else { acceleration.y = (b2Min(_direction.y * (_maxVelocity.y + _defaultVelocity.y), velocity.y + 8.5f) - velocity.y); } } else { acceleration.y = (_defaultVelocity.y - velocity.y); } force = _body->GetMass() * acceleration; _body->ApplyLinearImpulse(force, _body->GetWorldCenter(), true); float currentSpeed = velocity.Length(); if(currentSpeed > maxSpeed && _direction.y != 0) { _body->SetLinearVelocity((maxSpeed / currentSpeed) * velocity); } /* BIND PLAYER WITHIN THE VIEWPORT */ //calculate next step position glm::vec2 nextPosition = glm::vec2( _body->GetPosition().x + _body->GetLinearVelocity().x * deltaTime + (acceleration.x * deltaTime * deltaTime) / 2, _body->GetPosition().y + _body->GetLinearVelocity().y * deltaTime + (acceleration.y * deltaTime * deltaTime) / 2 ); glm::vec2 viewportSize = _game->getGameplayScreen()->getMainCamera()->getWorldViewportSize(); glm::vec2 cameraPosition = _game->getGameplayScreen()->getMainCamera()->getPosition() / _game->getGameplayScreen()->getMainCamera()->getZoom(); b2Vec2 correctedPosition = _body->GetPosition(); b2Vec2 correctionAcceleration = b2Vec2(0.0f, 0.0f); b2Vec2 currentVelocity = _body->GetLinearVelocity(); bool doCorrectPosition = false; if(nextPosition.x - _width / 2 < cameraPosition.x - viewportSize.x / 2) { correctedPosition.x = cameraPosition.x - viewportSize.x / 2 + _width / 2; correctionAcceleration.x = 0.0f - currentVelocity.x; doCorrectPosition = true; } if(nextPosition.x + _width / 2 > cameraPosition.x + viewportSize.x / 2) { correctedPosition.x = cameraPosition.x + viewportSize.x / 2 - _width / 2; correctionAcceleration.x = 0.0f - currentVelocity.x; doCorrectPosition = true; } if(nextPosition.y - _height / 2 < cameraPosition.y - viewportSize.y / 2 && _direction.y != 1) { correctedPosition.y = cameraPosition.y - viewportSize.y / 2 + _height / 2; correctionAcceleration.y = 0.0f - currentVelocity.y; doCorrectPosition = true; } if(nextPosition.y + _height / 2 > cameraPosition.y + viewportSize.y / 2 && _direction.y != -1) { correctedPosition.y = cameraPosition.y + viewportSize.y / 2 - _height / 2; correctionAcceleration.y = _defaultVelocity.y * 0.99f - currentVelocity.y; doCorrectPosition = true; } //if we have corrections to do, we must make sure to stop the body's velocity in the corrected direction as well. //still can be a bit weird... could be interpolated camera position or something... if(doCorrectPosition) { b2Vec2 force = _body->GetMass() * correctionAcceleration; //the impulse is applied in order to stop the body from moving further in that direction. _body->ApplyLinearImpulse(force, _body->GetWorldCenter(), true); _body->SetTransform(correctedPosition, _body->GetAngle()); } //Update Projectile Spawners _projectileSpawnerLeftPosition = glm::vec2(-_width / 2 + 0.5f, 0.1f); _projectileSpawnerRightPosition = _projectileSpawnerLeftPosition + glm::vec2(_width - 1.0f, 0.0f); correctProjectileSpawnersPosition(_animationManager->getCurrent()->getCurrentFrame()); int projectilesSpawnedLeft = _projectileSpawnerLeft.update(deltaTime, _isFiring, Utils::toVec2(_body->GetPosition()) + _projectileSpawnerLeftPosition + glm::vec2(0.0f, 1.0f), glm::vec2(0.0f, 1.0f), _body->GetAngle()); int projectilesSpawnedRight = _projectileSpawnerRight.update(deltaTime, _isFiring, Utils::toVec2(_body->GetPosition()) + _projectileSpawnerRightPosition + glm::vec2(0.0f, 1.0f), glm::vec2(0.0f, 1.0f), _body->GetAngle()); _game->getGameplayScreen()->addShotsFired(projectilesSpawnedLeft + projectilesSpawnedRight); if(projectilesSpawnedLeft > 0) { _muzzleLeftAnimationManager->play("MUZZLE"); _muzzleLeftAnimationManager->getCurrent()->reset(); _muzzleRightAnimationManager->play("MUZZLE"); _muzzleRightAnimationManager->getCurrent()->reset(); } //Update Animations _animationManager->update(deltaTime); _thrusterAnimationManager->update(deltaTime); _muzzleLeftAnimationManager->update(deltaTime); _muzzleRightAnimationManager->update(deltaTime); return true; }
// Perform a left or right rotation if node A is imbalanced. // Returns the new root index. int32 b2DynamicTree::Balance(int32 iA) { b2Assert(iA != b2_nullNode); b2TreeNode* A = m_nodes + iA; if (A->IsLeaf() || A->height < 2) { return iA; } int32 iB = A->child1; int32 iC = A->child2; b2Assert(0 <= iB && iB < m_nodeCapacity); b2Assert(0 <= iC && iC < m_nodeCapacity); b2TreeNode* B = m_nodes + iB; b2TreeNode* C = m_nodes + iC; int32 balance = C->height - B->height; // Rotate C up if (balance > 1) { int32 iF = C->child1; int32 iG = C->child2; b2TreeNode* F = m_nodes + iF; b2TreeNode* G = m_nodes + iG; b2Assert(0 <= iF && iF < m_nodeCapacity); b2Assert(0 <= iG && iG < m_nodeCapacity); // Swap A and C C->child1 = iA; C->parent = A->parent; A->parent = iC; // A's old parent should point to C if (C->parent != b2_nullNode) { if (m_nodes[C->parent].child1 == iA) { m_nodes[C->parent].child1 = iC; } else { b2Assert(m_nodes[C->parent].child2 == iA); m_nodes[C->parent].child2 = iC; } } else { m_root = iC; } // Rotate if (F->height > G->height) { C->child2 = iF; A->child2 = iG; G->parent = iA; A->aabb.Combine(B->aabb, G->aabb); C->aabb.Combine(A->aabb, F->aabb); A->height = 1 + b2Max(B->height, G->height); C->height = 1 + b2Max(A->height, F->height); } else { C->child2 = iG; A->child2 = iF; F->parent = iA; A->aabb.Combine(B->aabb, F->aabb); C->aabb.Combine(A->aabb, G->aabb); A->height = 1 + b2Max(B->height, F->height); C->height = 1 + b2Max(A->height, G->height); } return iC; } // Rotate B up if (balance < -1) { int32 iD = B->child1; int32 iE = B->child2; b2TreeNode* D = m_nodes + iD; b2TreeNode* E = m_nodes + iE; b2Assert(0 <= iD && iD < m_nodeCapacity); b2Assert(0 <= iE && iE < m_nodeCapacity); // Swap A and B B->child1 = iA; B->parent = A->parent; A->parent = iB; // A's old parent should point to B if (B->parent != b2_nullNode) { if (m_nodes[B->parent].child1 == iA) { m_nodes[B->parent].child1 = iB; } else { b2Assert(m_nodes[B->parent].child2 == iA); m_nodes[B->parent].child2 = iB; } } else { m_root = iB; } // Rotate if (D->height > E->height) { B->child2 = iD; A->child1 = iE; E->parent = iA; A->aabb.Combine(C->aabb, E->aabb); B->aabb.Combine(A->aabb, D->aabb); A->height = 1 + b2Max(C->height, E->height); B->height = 1 + b2Max(A->height, D->height); } else { B->child2 = iE; A->child1 = iD; D->parent = iA; A->aabb.Combine(C->aabb, D->aabb); B->aabb.Combine(A->aabb, E->aabb); A->height = 1 + b2Max(C->height, D->height); B->height = 1 + b2Max(A->height, E->height); } return iB; } return iA; }
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; float32 linearError = 0.0f; if (m_state == e_atUpperLimit) { b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetTransform().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->GetTransform().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->GetTransform().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; }
void b2DynamicTree::RebuildBottomUp() { int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); int32 count = 0; // Build array of leaves. Free the rest. for (int32 i = 0; i < m_nodeCapacity; ++i) { if (m_nodes[i].height < 0) { // free node in pool continue; } if (m_nodes[i].IsLeaf()) { m_nodes[i].parent = b2_nullNode; nodes[count] = i; ++count; } else { FreeNode(i); } } while (count > 1) { float32 minCost = b2_maxFloat; int32 iMin = -1, jMin = -1; for (int32 i = 0; i < count; ++i) { b2AABB aabbi = m_nodes[nodes[i]].aabb; for (int32 j = i + 1; j < count; ++j) { b2AABB aabbj = m_nodes[nodes[j]].aabb; b2AABB b; b.Combine(aabbi, aabbj); float32 cost = b.GetPerimeter(); if (cost < minCost) { iMin = i; jMin = j; minCost = cost; } } } int32 index1 = nodes[iMin]; int32 index2 = nodes[jMin]; b2TreeNode* child1 = m_nodes + index1; b2TreeNode* child2 = m_nodes + index2; int32 parentIndex = AllocateNode(); b2TreeNode* parent = m_nodes + parentIndex; parent->child1 = index1; parent->child2 = index2; parent->height = 1 + b2Max(child1->height, child2->height); parent->aabb.Combine(child1->aabb, child2->aabb); parent->parent = b2_nullNode; child1->parent = parentIndex; child2->parent = parentIndex; nodes[jMin] = nodes[count-1]; nodes[iMin] = parentIndex; --count; } m_root = nodes[0]; b2Free(nodes); Validate(); }
bool b2LineJoint::SolvePositionConstraints(float32 baumgarte) { B2_NOT_USED(baumgarte); b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; 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_localCenterA); b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB); 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); b2Vec2 impulse; float32 C1; C1 = b2Dot(m_perp, d); linearError = b2Max(linearError, b2Abs(C1)); angularError = 0.0f; if (active) { float32 m1 = m_invMassA, m2 = m_invMassB; float32 i1 = m_invIA, i2 = m_invIB; float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; m_K.col1.Set(k11, k12); m_K.col2.Set(k12, k22); b2Vec2 C; C.x = C1; C.y = C2; impulse = m_K.Solve(-C); } else { float32 m1 = m_invMassA, m2 = m_invMassB; float32 i1 = m_invIA, i2 = m_invIB; float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; float32 impulse1; if (k11 != 0.0f) { impulse1 = - C1 / k11; } else { impulse1 = 0.0f; } impulse.x = impulse1; impulse.y = 0.0f; } b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis; float32 L1 = impulse.x * m_s1 + impulse.y * m_a1; float32 L2 = impulse.x * m_s2 + impulse.y * m_a2; c1 -= m_invMassA * P; a1 -= m_invIA * L1; c2 += m_invMassB * P; a2 += m_invIB * 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; }
void Keyboard(unsigned char key, int x, int y){ B2_NOT_USED(x); B2_NOT_USED(y); switch (key) { case 27: #ifndef __APPLE__ // freeglut specific function glutLeaveMainLoop(); #endif 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 simulatorPage; simulatorPage = entry->createFcn(communicator); break; // Press space to launch a bomb. case ' ': if (simulatorPage) { simulatorPage->LaunchBomb(); } break; case 'p': settings.pause = !settings.pause; break; // Press [ to prev simulatorPage. case '[': --simulatorPageSelection; if (simulatorPageSelection < 0) { simulatorPageSelection = simulatorPageCount - 1; } glui->sync_live(); break; // Press ] to next simulatorPage. case ']': ++simulatorPageSelection; if (simulatorPageSelection == simulatorPageCount) { simulatorPageSelection = 0; } glui->sync_live(); break; default: if (simulatorPage) { simulatorPage->Keyboard(key); } } }