//------------------------------------------------------------------------------------------------
void Vehicle::applyAntiSwayBarForces ( )
{
Real amt;
Body * wheelBody;
for( int i = 0; i < 4; i++)
{
SuspensionJoint * const wheelJoint = static_cast<SuspensionJoint*>( _wheels.at( i )->getBody()->getJoint(0) );
const Ogre::Vector3 anchor2 (wheelJoint->getAdditionalAnchor());
const Ogre::Vector3 anchor1 (wheelJoint->getAnchor());
const Ogre::Vector3 axis (wheelJoint->getAxis());
const Ogre::Real displacement = ( anchor1 - anchor2 ).dotProduct( axis );
if( displacement > 0 )
{
amt = displacement * _swayForce;
if( amt > _swayForceLimit )
amt = _swayForceLimit;
// force down
wheelBody = _wheels.at( i )->getBody();
wheelBody->addForce( -axis * amt );
// force up
wheelBody = _wheels.at( i^1 )->getBody();
wheelBody->addForce( axis * amt );
}
}
}
void getBodies(Body &body, double error) {
double s = _box.getSize();
double d = (getCenterOfMass() - body.getPos()).getMagnitude();
double SbyD = s/d;
// cout << "S/D: " << SbyD << endl;
if (!isExternal()) {
if (SbyD < error) {
Body overallBody(_mass, 0, 0, getCenterOfMass().getX(), getCenterOfMass().getY());
body.addForce(body.calculateForceFromBody(overallBody));
return;
} else {
if (_NW != NULL)
_NW->getBodies(body, error);
if (_NE != NULL)
_NE->getBodies(body, error);
if (_SW != NULL)
_SW->getBodies(body, error);
if (_SE != NULL)
_SE->getBodies(body, error);
}
} else {
Body overallBody(_mass, 0, 0, getCenterOfMass().getX(), getCenterOfMass().getY());
body.addForce(body.calculateForceFromBody(overallBody));
// forceBodies.push_back(*_body);
}
}
vector<Body> calculateForcesOnBodies(vector<Body> &oldBodies, double error) {
vector<Body> newBodies;
for (int i = 0; i < oldBodies.size(); i++) {
Body current (oldBodies[i]);
Job* _force0 = new ForceCalculation(_root0, current, error);
Job* _force1 = new ForceCalculation(_root1, current, error);
Job* _force2 = new ForceCalculation(_root2, current, error);
Job* _force3 = new ForceCalculation(_root3, current, error);
thread0->setJob(_force0);
thread1->setJob(_force1);
thread2->setJob(_force2);
thread3->setJob(_force3);
thread0->WakeUpThread();
thread1->WakeUpThread();
thread2->WakeUpThread();
thread3->WakeUpThread();
while (!isIdle());
current.addForce(((ForceCalculation*)_force0)->getResult() +
((ForceCalculation*)_force1)->getResult() +
((ForceCalculation*)_force2)->getResult() +
((ForceCalculation*)_force3)->getResult());
newBodies.push_back(current);
delete _force0, _force1, _force2, _force3;
}
return newBodies;
}
