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