void WindModificator::applyImpulse (b2Body* body, b2Vec2 contactPoint, float force) const
{
const b2Vec2 direction = body->GetPosition() - getPos();
const float distance = 0.1f * b2Distance(body->GetPosition(), getPos());
const float impulseMag = std::min(70.0f, force * _modificatorSize / distance);
body->ApplyLinearImpulse(impulseMag * direction, contactPoint, true);
}
void b2Rope::Initialize(const b2RopeDef* def)
{
b2Assert(def->count >= 3);
m_count = def->count;
m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
m_ims = (float32*)b2Alloc(m_count * sizeof(float32));
for (int32 i = 0; i < m_count; ++i)
{
m_ps[i] = def->vertices[i];
m_p0s[i] = def->vertices[i];
m_vs[i].SetZero();
float32 m = def->masses[i];
if (m > 0.0f)
{
m_ims[i] = 1.0f / m;
}
else
{
m_ims[i] = 0.0f;
}
}
int32 count2 = m_count - 1;
int32 count3 = m_count - 2;
m_Ls = (float32*)b2Alloc(count2 * sizeof(float32));
m_as = (float32*)b2Alloc(count3 * sizeof(float32));
for (int32 i = 0; i < count2; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i+1];
m_Ls[i] = b2Distance(p1, p2);
}
for (int32 i = 0; i < count3; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i + 1];
b2Vec2 p3 = m_ps[i + 2];
b2Vec2 d1 = p2 - p1;
b2Vec2 d2 = p3 - p2;
float32 a = b2Cross(d1, d2);
float32 b = b2Dot(d1, d2);
m_as[i] = b2Atan2(a, b);
}
m_gravity = def->gravity;
m_damping = def->damping;
m_k2 = def->k2;
m_k3 = def->k3;
}
void UniteAerienne::Update()
{
if(m_timerSurfauffe.getElapsedTime().asSeconds()>=m_maxShootTime&&!m_surchauffe&&m_fire)
{
m_surchauffe=true;
m_timerSurfauffe.restart();
}
else if(m_surchauffe && m_timerSurfauffe.getElapsedTime().asSeconds()>=m_maxShootTime*3)
{
m_surchauffe=false;
m_timerSurfauffe.restart();
}
if(m_body->GetPosition().y>150)
m_body->SetGravityScale(5);
else if(b2Distance(m_body->GetLinearVelocity(), b2Vec2(0,0))<m_minVelocity||(!m_forces.avant&&!m_forces.arriere))
m_body->SetGravityScale(1);
else
m_body->SetGravityScale(0);
b2Vec2 vec(0,0);
if(m_forces.avant)
{
sf::Transform tr;
tr.rotate(Trigo::ToDeg(m_body->GetAngle()));
sf::Vector2f v;
if(m_sens==1)
{
v=tr.transformPoint(m_acceleration, 0);
m_AddTrainee();
}
else
v=tr.transformPoint(m_acceleration*0.05, 0);
vec.Set(v.x, v.y);
m_body->ApplyForceToCenter(vec);
}
if(m_forces.arriere)
{
sf::Transform tr;
tr.rotate(Trigo::ToDeg(m_body->GetAngle()));
sf::Vector2f v;
if(m_sens==-1)
{
v=tr.transformPoint(-m_acceleration, 0);
m_AddTrainee();
}
else
v=tr.transformPoint(-m_acceleration*0.05, 0);
vec.Set(v.x, v.y);
m_body->ApplyForceToCenter(vec);
}
if(m_forces.monte)
m_body->ApplyAngularImpulse(m_rotationVelocity);
else if(m_forces.descend)
m_body->ApplyAngularImpulse(-m_rotationVelocity);
Unite::Update();
}
bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
const b2Transform& xfA, const b2Transform& xfB)
{
b2DistanceInput input;
input.proxyA.Set(shapeA);
input.proxyB.Set(shapeB);
input.transformA = xfA;
input.transformB = xfB;
input.useRadii = true;
b2SimplexCache cache;
cache.count = 0;
b2DistanceOutput output;
b2Distance(&output, &cache, &input);
return output.distance < 10.0f * b2_epsilon;
}
// this function returns -1 if two objects has collision
// And otherwise returns the distance
float PlanningProblem::distToObstacle(Station A, const Obstacle &ob, b2Vec2& A_point, b2Vec2& ob_point)
{
b2DistanceProxy state_proxy, ob_proxy;
state_proxy.Set(agent->shape, 0);
ob_proxy.Set(ob.shape, 1);
b2DistanceInput dist_in;
dist_in.proxyA = state_proxy;
dist_in.proxyB = ob_proxy;
dist_in.transformA = b2Transform(A.getPosition().toB2vec2(),
b2Rot(A.getPosition().Teta()));
dist_in.transformB = ob.transform;
b2SimplexCache dist_cache;
dist_cache.count = 0;
b2DistanceOutput dis_out;
b2Distance(&dis_out, &dist_cache, &dist_in);
A_point = dis_out.pointA;
ob_point = dis_out.pointB;
if(hasCollision(A, ob)) {
return -1;
}
return dis_out.distance;
}
float32 GetMetric() const
{
switch (m_count)
{
case 0:
b2Assert(false);
return 0.0;
case 1:
return 0.0f;
case 2:
return b2Distance(m_v1.w, m_v2.w);
case 3:
return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
default:
b2Assert(false);
return 0.0f;
}
}
void VRope::initWithBodysAndRopeLenght(b2World* world, b2Body *body1, b2Body *body2, cocos2d::CCSpriteBatchNode *spriteSheetArg, float ropeLength){
bodyA = body1;
bodyB = body2;
mWorld = world;
float length = ropeLength; //
if (length < 0)
length = b2Distance(body1->GetPosition(), body2->GetPosition());
//创建关节
b2RopeJointDef jd;
jd.bodyA=bodyA;
jd.bodyB=bodyB;
jd.localAnchorA = b2Vec2(0,0);
jd.localAnchorB = b2Vec2(0,0);
jd.maxLength= length;
mJoint = (b2RopeJoint*)mWorld->CreateJoint(&jd);
CCPoint pointA = ccp(bodyA->GetPosition().x*PTM_RATIO,bodyA->GetPosition().y*PTM_RATIO);
CCPoint pointB = ccp(bodyB->GetPosition().x*PTM_RATIO,bodyB->GetPosition().y*PTM_RATIO);
spriteSheet = spriteSheetArg;
createRope(pointA, pointB, (length+0.1)*PTM_RATIO);
}
void VehicleMonitor::WriteCSV(double elapsedTime)
{
std::ostringstream row;
//events
if (m_IsColliding == true)
row << "C:" << m_obstacleName << ";";
else
row << ",";
row << elapsedTime << ",";
row << m_simObject->GetPosition().x << ",";
row << m_simObject->GetPosition().y << ",";
row << ((Vehicle*)m_simObject)->m_body->GetAngle() << ",";
//internal data
for (auto& datum : ((Vehicle*)m_simObject)->GetInternalData())
{
row << datum.second << ",";
}
row.str().pop_back();
m_csvStream << row.str() << "\n";
m_dist_travelled += b2Distance(prevPos, m_simObject->GetPosition());
prevPos = m_simObject->GetPosition();
}
void VRope::initWithBodys(b2World* world, b2Body *body1, b2Body *body2, CCSpriteBatchNode *spriteSheetArg) {
initWithBodysAndRopeLenght(world, body1, body2, spriteSheetArg, b2Distance(body1->GetPosition(), body2->GetPosition()));
}
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;
}
}
}
void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
{
b2Assert(toiIndexA < m_bodyCount);
b2Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
m_positions[i].c = b->m_sweep.c;
m_positions[i].a = b->m_sweep.a;
m_velocities[i].v = b->m_linearVelocity;
m_velocities[i].w = b->m_angularVelocity;
}
b2ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.allocator = m_allocator;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver(&contactSolverDef);
// Solve position constraints.
for (int32 i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if 0
// Is the new position really safe?
for (int32 i = 0; i < m_contactCount; ++i)
{
b2Contact* c = m_contacts[i];
b2Fixture* fA = c->GetFixtureA();
b2Fixture* fB = c->GetFixtureB();
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
int32 indexA = c->GetChildIndexA();
int32 indexB = c->GetChildIndexB();
b2DistanceInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.transformA = bA->GetTransform();
input.transformB = bB->GetTransform();
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache;
cache.count = 0;
b2Distance(&output, &cache, &input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int32 i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float32 h = subStep.dt;
// Integrate positions
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float32 a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float32 w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
float32 ratio = b2_maxTranslation / translation.Length();
v *= ratio;
}
float32 rotation = h * w;
if (rotation * rotation > b2_maxRotationSquared)
{
float32 ratio = b2_maxRotation / b2Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
b2Body* body = m_bodies[i];
body->m_sweep.c = c;
body->m_sweep.a = a;
body->m_linearVelocity = v;
body->m_angularVelocity = w;
body->SynchronizeTransform();
}
Report(contactSolver.m_velocityConstraints);
}
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 b2Island::SolveTOI(const b2TimeStep& subStep, const b2Body* bodyA, const b2Body* bodyB)
{
b2ContactSolverDef solverDef;
solverDef.contacts = m_contacts;
solverDef.count = m_contactCount;
solverDef.allocator = m_allocator;
solverDef.impulseRatio = subStep.dtRatio;
solverDef.warmStarting = subStep.warmStarting;
b2ContactSolver contactSolver(&solverDef);
// Solve position constraints.
const float32 k_toiBaumgarte = 0.75f;
for (int32 i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(k_toiBaumgarte, bodyA, bodyB);
if (contactsOkay)
{
break;
}
if (i == subStep.positionIterations - 1)
{
i += 0;
}
}
#if 0
// Is the new position really safe?
for (int32 i = 0; i < m_contactCount; ++i)
{
b2Contact* c = m_contacts[i];
b2Fixture* fA = c->GetFixtureA();
b2Fixture* fB = c->GetFixtureB();
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
int32 indexA = c->GetChildIndexA();
int32 indexB = c->GetChildIndexB();
b2DistanceInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.transformA = bA->GetTransform();
input.transformB = bB->GetTransform();
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache;
cache.count = 0;
b2Distance(&output, &cache, &input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
for (int32 i = 0; i < m_bodyCount; ++i)
{
m_bodies[i]->m_sweep.a0 = m_bodies[i]->m_sweep.a;
m_bodies[i]->m_sweep.c0 = m_bodies[i]->m_sweep.c;
}
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int32 i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
// Check for large velocities.
b2Vec2 translation = subStep.dt * b->m_linearVelocity;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
translation.Normalize();
b->m_linearVelocity = (b2_maxTranslation * subStep.inv_dt) * translation;
}
float32 rotation = subStep.dt * b->m_angularVelocity;
if (rotation * rotation > b2_maxRotationSquared)
{
if (rotation < 0.0)
{
b->m_angularVelocity = -subStep.inv_dt * b2_maxRotation;
}
else
{
b->m_angularVelocity = subStep.inv_dt * b2_maxRotation;
}
}
// Integrate
b->m_sweep.c += subStep.dt * b->m_linearVelocity;
b->m_sweep.a += subStep.dt * b->m_angularVelocity;
// Compute new transform
b->SynchronizeTransform();
// Note: shapes are synchronized later.
}
Report(contactSolver.m_constraints);
}
// 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);
}
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;
}
bool b2Conservative(b2Shape* shape1, b2Shape* shape2)
{
b2Body* body1 = shape1->GetBody();
b2Body* body2 = shape2->GetBody();
b2Vec2 v1 = body1->m_position - body1->m_position0;
float32 omega1 = body1->m_rotation - body1->m_rotation0;
b2Vec2 v2 = body2->m_position - body2->m_position0;
float32 omega2 = body2->m_rotation - body2->m_rotation0;
float32 r1 = shape1->GetMaxRadius();
float32 r2 = shape2->GetMaxRadius();
b2Vec2 p1Start = body1->m_position0;
float32 a1Start = body1->m_rotation0;
b2Vec2 p2Start = body2->m_position0;
float32 a2Start = body2->m_rotation0;
b2Vec2 p1 = p1Start;
float32 a1 = a1Start;
b2Vec2 p2 = p2Start;
float32 a2 = a2Start;
b2Mat22 R1(a1), R2(a2);
shape1->QuickSync(p1, R1);
shape2->QuickSync(p2, R2);
float32 s1 = 0.0f;
const int32 maxIterations = 10;
b2Vec2 d;
float32 invRelativeVelocity = 0.0f;
bool hit = true;
b2Vec2 x1, x2;
for (int32 iter = 0; iter < maxIterations; ++iter)
{
// Get the accurate distance between shapes.
float32 distance = b2Distance(&x1, &x2, shape1, shape2);
if (distance < b2_linearSlop)
{
if (iter == 0)
{
hit = false;
}
else
{
hit = true;
}
break;
}
if (iter == 0)
{
b2Vec2 d = x2 - x1;
d.Normalize();
float32 relativeVelocity = b2Dot(d, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
if (b2Abs(relativeVelocity) < FLT_EPSILON)
{
hit = false;
break;
}
invRelativeVelocity = 1.0f / relativeVelocity;
}
// Get the conservative movement.
float32 ds = distance * invRelativeVelocity;
float32 s2 = s1 + ds;
if (s2 < 0.0f || 1.0f < s2)
{
hit = false;
break;
}
if (s2 < (1.0f + 100.0f * FLT_EPSILON) * s1)
{
hit = true;
break;
}
s1 = s2;
// Move forward conservatively.
p1 = p1Start + s1 * v1;
a1 = a1Start + s1 * omega1;
p2 = p2Start + s1 * v2;
a2 = a2Start + s1 * omega2;
R1.Set(a1);
R2.Set(a2);
shape1->QuickSync(p1, R1);
shape2->QuickSync(p2, R2);
}
if (hit)
{
// Hit, move bodies to safe position and re-sync shapes.
b2Vec2 d = x2 - x1;
float32 length = d.Length();
if (length > FLT_EPSILON)
{
d *= b2_linearSlop / length;
}
if (body1->IsStatic())
{
body1->m_position = p1;
}
else
{
body1->m_position = p1 - d;
}
body1->m_rotation = a1;
body1->m_R.Set(a1);
body1->QuickSyncShapes();
if (body2->IsStatic())
{
body2->m_position = p2;
}
else
{
body2->m_position = p2 + d;
}
body2->m_position = p2 + d;
body2->m_rotation = a2;
body2->m_R.Set(a2);
body2->QuickSyncShapes();
return true;
}
// No hit, restore shapes.
shape1->QuickSync(body1->m_position, body1->m_R);
shape2->QuickSync(body2->m_position, body2->m_R);
return false;
}
DirectSprite* EngineBase::add_sprite(int model, float x, float y, void* userdata)
{
DirectSprite* sp = new DirectSprite;
//animations
sp->set_num = SpriteModel::instance()->Models[model].subset_num;
sp->animes = SpriteModel::instance()->Models[model].animes;
//sp->texco_trans = { XMFLOAT2(0, 0), XMFLOAT2(1, 1) };
if (!sp->animes.empty())
{
sp->anime_active = true;
sp->current_anim = SpriteModel::instance()->Models[model].defanime;
sp->distH = SpriteModel::instance()->Models[model].disth;
sp->distV = SpriteModel::instance()->Models[model].distv;
sp->_interval = SpriteModel::instance()->Models[model].interval;
Animated_Sprites.push_back(sp);
}
else
{
sp->anime_active = false;
}
//box2d
{
b2BodyDef bdef;
bdef.position.Set(x * tometer, y * tometer);
bdef.type = SpriteModel::instance()->Models[model].bodytype;
sp->body = tworld->CreateBody(&bdef);
}
{
b2FixtureDef fdef;
fdef.density = 1;
fdef.friction = 1;
if (SpriteModel::instance()->Models[model].shapetype.size() !=
SpriteModel::instance()->Models[model].vert_colli.size())
{
MessageBox(0, "error fixture num!", 0, 0);
return nullptr;
}
for (int i = 0; i < SpriteModel::instance()->Models[model].shapetype.size(); i++)
{
if (SpriteModel::instance()->Models[model].shapetype[i] == b2Shape::Type::e_polygon)
{
b2PolygonShape shape;
vector<b2Vec2> vert = SpriteModel::instance()->Models[model].vert_colli[i];
for (int ii = 0; ii < vert.size(); ii++)
{
vert[ii] = tometer * vert[ii];
}
shape.Set(&vert[0], vert.size());
fdef.shape = &shape;
sp->body->CreateFixture(&fdef);
}
else if (SpriteModel::instance()->Models[model].shapetype[i] == b2Shape::Type::e_closeedge)
{
b2EdgeShape shape;
vector<b2Vec2> vert = SpriteModel::instance()->Models[model].vert_colli[i];
for (int ii = 0; ii < vert.size(); ii++)
{
vert[ii] = tometer * vert[ii];
}
for (int ii = 0; ii < vert.size(); ii++)
{
int v0 = ii - 1 < 0 ? vert.size() - 1 : ii - 1;
int v1 = ii;
int v2 = ii + 1 >= vert.size() ? 0 : ii + 1;
int v3 = v2 + 1 >= vert.size() ? 0 : v2 + 1;
shape.Set(vert[v1], vert[v2]);
shape.m_hasVertex0 = true;
shape.m_hasVertex3 = true;
shape.m_vertex0 = vert[v0];
shape.m_vertex3 = vert[v3];
fdef.shape = &shape;
sp->body->CreateFixture(&fdef);
}
}
else if (SpriteModel::instance()->Models[model].shapetype[i] == b2Shape::Type::e_edge)
{
b2EdgeShape shape;
vector<b2Vec2> vert = SpriteModel::instance()->Models[model].vert_colli[i];
for (int ii = 0; ii < vert.size(); ii++)
{
vert[ii] = tometer * vert[ii];
}
for (int ii = 0; ii < vert.size(); ii++)
{
int v0 = ii - 1 < 0 ? vert.size() - 1 : ii - 1;
int v1 = ii;
int v2 = ii + 1 >= vert.size() ? 0 : ii + 1;
int v3 = v2 + 1 >= vert.size() ? 0 : v2 + 1;
if (ii == 0)
{
shape.Set(vert[v1], vert[v2]);
shape.m_hasVertex3 = true;
shape.m_vertex3 = vert[v3];
fdef.shape = &shape;
sp->body->CreateFixture(&fdef);
}
else if (ii == vert.size() - 1)
{
shape.Set(vert[v1], vert[v2]);
shape.m_hasVertex0 = true;
shape.m_vertex0 = vert[v0];
fdef.shape = &shape;
sp->body->CreateFixture(&fdef);
}
else
{
shape.Set(vert[v1], vert[v2]);
shape.m_hasVertex0 = true;
shape.m_hasVertex3 = true;
shape.m_vertex0 = vert[v0];
shape.m_vertex3 = vert[v3];
fdef.shape = &shape;
sp->body->CreateFixture(&fdef);
}
}
}
else if (SpriteModel::instance()->Models[model].shapetype[i] == b2Shape::Type::e_circle)
{
b2CircleShape shape;
shape.m_p = b2Vec2(0, 0);
shape.m_radius = tometer * b2Distance(
b2Vec2(
SpriteModel::instance()->Models[model].vert_colli[i][0].x,
SpriteModel::instance()->Models[model].vert_colli[i][0].y),
b2Vec2(
SpriteModel::instance()->Models[model].vert_colli[i][1].x,
SpriteModel::instance()->Models[model].vert_colli[i][1].y));
fdef.shape = &shape;
sp->body->CreateFixture(&fdef);
}
}
}
if (userdata != nullptr)
{
sp->body->SetUserData(userdata);
}
//update world matrix!
sp->update_world_matrix();
if (sp->body->GetType() == b2BodyType::b2_staticBody) //kinematic body can also be static! fix this! don't forget!
{
Static_Sprites.push_back(sp);
}
else
{
Dynamic_Sprites.push_back(sp);
}
return sp;
}
bool UniteAerienne::SubirDegatsTerrain()
{
if(b2Distance(m_body->GetLinearVelocity(), b2Vec2(0,0))>20)
SubirDegats(100000);
return EstVivant();
}
// This algorithm uses conservative advancement to compute the time of
// impact (TOI) of two shapes.
// Refs: Bullet, Young Kim
float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
const b2Shape* shape2, const b2Sweep& sweep2)
{
float32 r1 = shape1->GetSweepRadius();
float32 r2 = shape2->GetSweepRadius();
b2Assert(sweep1.t0 == sweep2.t0);
b2Assert(1.0f - sweep1.t0 > B2_FLT_EPSILON);
float32 t0 = sweep1.t0;
b2Vec2 v1 = sweep1.c - sweep1.c0;
b2Vec2 v2 = sweep2.c - sweep2.c0;
float32 omega1 = sweep1.a - sweep1.a0;
float32 omega2 = sweep2.a - sweep2.a0;
float32 alpha = 0.0f;
b2Vec2 p1, p2;
const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
int32 iter = 0;
b2Vec2 normal = b2Vec2_zero;
float32 distance = 0.0f;
float32 targetDistance = 0.0f;
for(;;)
{
float32 t = (1.0f - alpha) * t0 + alpha;
b2XForm xf1, xf2;
sweep1.GetXForm(&xf1, t);
sweep2.GetXForm(&xf2, t);
// Get the distance between shapes.
distance = b2Distance(&p1, &p2, shape1, xf1, shape2, xf2);
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement.
if (distance > 2.0f * b2_toiSlop)
{
targetDistance = 1.5f * b2_toiSlop;
}
else
{
targetDistance = b2Max(0.05f * b2_toiSlop, distance - 0.5f * b2_toiSlop);
}
}
if (distance - targetDistance < 0.05f * b2_toiSlop || 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;
}
return alpha;
}
void MainScene::CreateAsteroids()
{
Vec2 center(0,0);
const string names[] =
{
"Asteroid_01",
"Asteroid_02",
"Asteroid_03",
"Asteroid_04",
"Asteroid_05",
"Asteroid_06",
"Asteroid_07",
// "Asteroid_08",
};
const int MAX_NAMES = sizeof(names)/sizeof(names[0]);
typedef struct
{
float32 radius;
uint32 asteroids;
} RING_DATA_T;
RING_DATA_T ringData[] =
{
{1, 1},
{9, 3},
{17, 7},
{25, 13},
{35, 20},
};
const int MAX_RINGS = sizeof(ringData)/sizeof(ringData[0]);
float32 angleRads = 0;
uint32 nameIdx = 0;
for(int ring = 0; ring < MAX_RINGS; ring++)
{
// Inner ring asteroids
float32 targetRadius = ringData[ring].radius;
uint32 asteroids = ringData[ring].asteroids;
for(int idx = 0; idx < asteroids; idx++)
{
Vec2 offset = Vec2::FromPolar(targetRadius, angleRads);
Vec2 position = center + offset;
Asteroid* asteroid = new Asteroid();
asteroid->Create(*_world,
names[nameIdx%MAX_NAMES],
position,
9.0 + RanNumGen::RandFloat(-1.0, 1.0));
asteroid->GetBody()->SetDebugDraw(false);
EntityManager::Instance().Register(asteroid);
_asteroids.push_back(asteroid);
// All asteroids have a distance joint to the anchor
// Now create the joint.
b2RopeJointDef jointDef;
jointDef.bodyA = _anchor;
jointDef.bodyB = asteroid->GetBody();
jointDef.maxLength = b2Distance(jointDef.bodyA->GetPosition(), jointDef.bodyB->GetPosition());
jointDef.collideConnected = true;
_world->CreateJoint(&jointDef);
nameIdx++;
angleRads += (2*M_PI)/asteroids + RanNumGen::RandFloat(-M_PI/(12*asteroids), M_PI/(12*asteroids));
}
}
for(int idx = 0;idx < _asteroids.size(); idx++)
{
_asteroidLayer->AddSprite(_asteroids[idx]->GetSprite());
EntityScheduler::Instance().Register(_asteroids[idx]);
// Give it at least one update to start.
_asteroids[idx]->Update();
}
}
