void btCollisionWorld::performDiscreteCollisionDetection()
{
btDispatcherInfo& dispatchInfo = getDispatchInfo();
BEGIN_PROFILE("perform Broadphase Collision Detection");
//update aabb (of all moved objects)
btVector3 aabbMin,aabbMax;
for (int i=0;i<m_collisionObjects.size();i++)
{
m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
}
m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
END_PROFILE("perform Broadphase Collision Detection");
BEGIN_PROFILE("performDiscreteCollisionDetection");
btDispatcher* dispatcher = getDispatcher();
if (dispatcher)
dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
END_PROFILE("performDiscreteCollisionDetection");
}
// Note that this differs in the calculation of sx, sy, sa
// from simuv2, but is exactly the same when everything is horizontal.
// I changed the implementation so that what happens is mathematically
// clearer, though the code is not very clear :/
void
SimWheelUpdateForce(tCar *car, int index)
{
tWheel *wheel = &(car->wheel[index]);
tdble axleFz = wheel->axleFz;
// tdble vt, v, v2;
tdble wrl; /* wheel related velocity */
tdble Fn, Ft, Ft2;
tdble waz;
tdble CosA, SinA;
tdble s = 0.0;
tdble sa, sx, sy; /* slip vector */
tdble stmp, F, Bx;
tdble mu;
tdble reaction_force;
tdble f_z = 0.0;
t3Dd angles;
t3Dd normal;
t3Dd rel_normal;
bool right_way_up = true;
static long wcnt = 0;
#ifdef USE_THICKNESS
int seg_id = (int) ((tdble) N_THICKNESS_SEGMENTS * (wheel->relPos.ay/(2*M_PI))) % N_THICKNESS_SEGMENTS;
if (seg_id<0) seg_id += N_THICKNESS_SEGMENTS;
tdble adjRadius = wheel->radius + wheel->thickness[seg_id];
#else
tdble adjRadius = wheel->radius;
#endif
wheel->T_current = car->carElt->_tyreT_mid(index);
wheel->condition = car->carElt->_tyreCondition(index);
waz = wheel->relPos.az;//wheel->steer + wheel->staticPos.az;
/* Get normal of road relative to the wheel's axis
This should help take into account the camber.*/
BEGIN_PROFILE(timer_coordinate_transform);
//RtTrackSurfaceNormalL(&(wheel->trkPos), &normal);
normal = wheel->normal;
// now rel_normal.x is the effective camber angle
if (USE_QUATERNIONS==0) {
angles.x = car->DynGCg.pos.ax + wheel->relPos.ax;
angles.y = car->DynGCg.pos.ay;
angles.z = car->DynGCg.pos.az + waz;
NaiveRotate (normal, angles, &rel_normal);
} else {
sgQuat Q;
sgCopyQuat (Q, car->posQuat);
sgPreRotQuat (Q, FLOAT_RAD2DEG(wheel->relPos.ax), 1.0f, 0.0f, 0.0f);
sgPreRotQuat (Q, FLOAT_RAD2DEG(waz), 0.0f, 0.0f, 1.0f);
sgVec3 P = {normal.x, normal.y, normal.z};
sgRotateVecQuat (P, Q);
sg2t3 (P, rel_normal);
}
wheel->state = 0;
END_PROFILE(timer_coordinate_transform);
BEGIN_PROFILE(timer_reaction);
Ft = 0.0;
Fn = 0.0;
wheel->forces.x = 0.0;
wheel->forces.y = 0.0;
wheel->forces.z = 0.0;
/* Now uses the normal, so it should work */
/* update suspension force */
SimSuspUpdate(&(wheel->susp));
/* check suspension state */
wheel->state |= wheel->susp.state;
reaction_force = 0.0;
wheel->forces.z = 0;
Ft = Fn = 0;
reaction_force = 0.0;
if ((wheel->state & SIM_SUSP_EXT) == 0) {
f_z = axleFz + wheel->susp.force;
wheel->rel_vel -= SimDeltaTime * wheel->susp.force / wheel->mass;
if ((f_z < 0)) {
f_z = 0;
}
/* project the reaction force. Only wheel->forces.z is
actually interesting for friction. The rest is just
reaction. Now we have included the reaction from the sides
which is fake.
The suspension pushes the wheel down with f_z, but the reaction
of the surface is just f_z*rel_normal.z;!
*/
if ((right_way_up) && (rel_normal.z > MIN_NORMAL_Z)) {
// reaction force on track z axis should be equal to
// suspension reaction if suspension is perpendicular
// to the track plane. We assume other reaction forces
// proportional, but things break down when car is
// tilted a lot with respect to the track plane.
tdble invrel_normal = 1.0f/rel_normal.z;
if (invrel_normal>= 4.0) {
invrel_normal = 4.0;
} else if (invrel_normal<=-4.0) {
invrel_normal = -4.0;
}
reaction_force = f_z; //* invrel_normal;
// the other reactions are then:
Ft = reaction_force*rel_normal.x;//*invrel_normal;
Fn = reaction_force*rel_normal.y;//*invrel_normal;
} else {
f_z = 0;
wheel->susp.force = 0;
wheel->forces.z = 0;
Ft = Fn = 0;
reaction_force = 0.0;
}
} else {
//if (wheel->rel_vel < 0.0) {
//wheel->rel_vel = 0.0;
//}
wheel->rel_vel -= SimDeltaTime * wheel->susp.force / wheel->mass;
wheel->forces.z = 0.0f;
}
wheel->relPos.z = - wheel->susp.x / wheel->susp.spring.bellcrank + adjRadius; /* center relative to GC */
END_PROFILE(timer_reaction);
BEGIN_PROFILE(timer_angles);
/* HORIZONTAL FORCES */
CosA = cos(waz);
SinA = sin(waz);
/* tangent velocity */
// This is speed of the wheel relative to the track, so we have
// to take the projection to the track.
tdble rel_normal_xz = sqrt (rel_normal.z*rel_normal.z
+ rel_normal.x*rel_normal.x);
tdble rel_normal_yz = sqrt (rel_normal.z*rel_normal.z
+ rel_normal.y*rel_normal.y);
//tdble rel_normal_xy = sqrt (rel_normal.x*rel_normal.x
// + rel_normal.y*rel_normal.y);
END_PROFILE(timer_angles);
#ifndef FREE_MOVING_WHEELS
wheel->bodyVel.z = 0.0;
#endif
wrl = (wheel->spinVel + car->DynGC.vel.ay) * adjRadius;
{
// this thing here should be faster than quat?
t3Dd angles = {wheel->relPos.ax, 0.0, waz};
NaiveRotate (wheel->bodyVel, angles, &wheel->bodyVel);
}
tdble wvx = wheel->bodyVel.x * rel_normal_yz;
tdble wvy = wheel->bodyVel.y * rel_normal_xz;
tdble absolute_speed = sqrt(wvx*wvx + wvy*wvy);
wvx -= wrl;
wheel->bodyVel.x = wvx;
wheel->bodyVel.y = wvy;
BEGIN_PROFILE(timer_friction);
tdble relative_speed = sqrt(wvx*wvx + wvy*wvy);
tdble camber_gain = +0.1f;
tdble camber_shift = camber_gain * rel_normal.x;
if ((wheel->state & SIM_SUSP_EXT) != 0) {
sx = sy = sa = 0;
} else if (absolute_speed < ABSOLUTE_SPEED_CUTOFF) {
sx = wvx/ABSOLUTE_SPEED_CUTOFF;
sy = wvy/ABSOLUTE_SPEED_CUTOFF;
sa = atan2(wvy, wvx);
} else {
// The division with absolute_speed is a bit of a hack.
// But the assumption is that the profile of friction
// scales linearly with speed.
sx = wvx/absolute_speed;
sy = wvy/absolute_speed;
sa = atan2(wvy, wvx);
}
s = sqrt(sx*sx+sy*sy);
sa -= camber_shift;
sx = cos(sa)*s;
sy = sin(sa)*s;
wcnt++;
access_times = (float) wcnt;
//if (index==0) {
//wcnt--;
//}
if (wcnt<0) {
//printf ("%f", reaction_force);
if (index==3) {
wcnt = 10;
//printf ("#RCT\n");
} else {
//printf (" ");
}
}
if (right_way_up) {
if (fabs(absolute_speed) < 2.0f && fabs(wrl) < 2.0f) {
car->carElt->_skid[index] = 0.0f;
} else {
car->carElt->_skid[index] = MIN(1.0f, (s*reaction_force*0.0002f));
}
//0.0002*(MAX(0.2, MIN(s, 1.2)) - 0.2)*reaction_force;
car->carElt->_reaction[index] = reaction_force;
} else {
car->carElt->_skid[index] = 0.0f;
car->carElt->_reaction[index] = 0.0f;
}
stmp = MIN(s, 1.5f);
/* MAGIC FORMULA */
Bx = wheel->mfB * stmp;
tdble dynamic_grip = wheel->mfT * sin(wheel->mfC * atan(Bx * (1 - wheel->mfE) + wheel->mfE * atan(Bx))) * (1.0f + stmp * simSkidFactor[car->carElt->_skillLevel]);
//printf ("%f\n", simSkidFactor[car->carElt->_skillLevel]);
/* load sensitivity */
mu = wheel->mu * (wheel->lfMin + (wheel->lfMax - wheel->lfMin) * exp(wheel->lfK * reaction_force / wheel->opLoad));
//mu = wheel->mu;
tdble static_grip = wheel->condition * reaction_force * mu * wheel->trkPos.seg->surface->kFriction;
//tdble static_grip = wheel->condition * reaction_force * mu * wheel->trkPos.seg->surface->kFriction/0.7f;
F = dynamic_grip * static_grip;
// This is the steering torque
{
tdble Bx = wheel->mfB * (sa);// + camber_shift);
//printf ("%f %f\n", sa, camber_shift);
car->carElt->_wheelFy(index) = (tdble)(cos(sa)*wheel->mfT * sin(wheel->mfC * atan(Bx * (1 - wheel->mfE) + wheel->mfE * atan(Bx))) * (1.0 + stmp * simSkidFactor[car->carElt->_skillLevel]) * static_grip);
}
END_PROFILE(timer_friction);
BEGIN_PROFILE(timer_temperature);
if (car->options->tyre_temperature) {
// heat transfer function with air
tdble htrf = (tdble)((0.002 + fabs(absolute_speed)*0.0005)*SimDeltaTime);
tdble T_current = wheel->T_current;
tdble T_operating = wheel->T_operating;
tdble T_range = wheel->T_range;
tdble mfT;
// friction heat transfer
T_current += (tdble)(0.00003*((fabs(relative_speed)+0.1*fabs(wrl))*reaction_force)*SimDeltaTime);
T_current = (tdble)(T_current * (1.0-htrf) + htrf * 25.0);
tdble dist = (T_current - T_operating)/T_range;
//mfT = 100.0f * exp(-0.5f*(dist*dist))/T_range;
mfT = 0.85f + 3.0f * exp(-0.5f*(dist*dist))/T_range;
if (T_current>200.0) T_current=200.0;
wheel->mfT = mfT;
wheel->T_current = T_current;
}
if (car->options->tyre_damage > 0.0f && s>0.01f) {
tdble compound_melt_point = wheel->T_operating + wheel->T_range;
tdble adherence = wheel->Ca * 500.0f;
tdble melt = (exp (2.0f*(wheel->T_current - compound_melt_point)/compound_melt_point)) * car->options->tyre_damage;
tdble removal = exp (2.0f*(F - adherence)/adherence);
tdble wheel_damage = (tdble)(0.001 * melt * relative_speed * removal / (2.0 * M_PI * wheel->radius * wheel->width * wheel->Ca));
if (wheel_damage>0.01f) {
wheel_damage = 0.01f;
}
tdble delta_dam = wheel_damage * SimDeltaTime;
wheel->condition -= 0.5f*delta_dam;
if (wheel->condition < 0.5f) wheel->condition = 0.5f;
} else {
wheel->mfT = 1.0f;
}
END_PROFILE(timer_temperature);
BEGIN_PROFILE(timer_force_calculation);
wheel->rollRes = reaction_force * wheel->trkPos.seg->surface->kRollRes;
car->carElt->priv.wheel[index].rollRes = wheel->rollRes;
// Calculate friction forces
Ft2 = 0.0f;
tdble Fn2 = 0.0f;
tdble epsilon = 0.00001f;
if (s > epsilon) {
/* wheel axis based - no control after an angle*/
if (rel_normal.z > MIN_NORMAL_Z) {
// When the tyre is tilted there is less surface
// touching the road. Modelling effect simply with rel_normal_xz.
// Constant 1.05f for equality with simuv2.
tdble sur_f = 1.05f * rel_normal_xz;
sur_f = 1.0;
Ft2 = - sur_f*F*sx/s;
Fn2 = - sur_f*F*sy/s;
} else {
Ft2 = 0.0f;
Fn2 = 0.0f;
}
//Ft2 -= camber_shift*F;
} else {
tdble sur_f = rel_normal_xz;
Ft2 = - sur_f*F*sx/epsilon;
Fn2 = - sur_f*F*sy/epsilon;
}
Ft2 -= tanh(wvx) * fabs(wheel->rollRes);
Fn2 -= tanh(wvy) * fabs(wheel->rollRes);
wheel->forces.x = Ft2 * rel_normal_yz;
wheel->forces.y = Fn2 * rel_normal_xz;
wheel->forces.z = Ft2 * rel_normal.x + Fn2 * rel_normal.y;
END_PROFILE(timer_force_calculation);
if (0) {
// EXPERIMENTAL code - estimate amount of mass linked to this
// wheel. Maybe useful for adjusting the slope of the
// static friction function. Currently not used.
tdble Ftot = sqrt(Ft2*Ft2 + Fn2*Fn2);
tdble ds = wheel->s_old-s;
tdble EF = wheel->Em * ds;
tdble dF = wheel->F_old - EF;
wheel->Em += (float)(0.1 * dF*ds);
wheel->F_old = Ftot;
wheel->s_old = s;
}
wheel->relPos.az = waz;
if (rel_normal.z > MIN_NORMAL_Z) {
right_way_up = true;
} else {
right_way_up = false;
}
if (car->collide_timer < 0.00) {
right_way_up = false;
}
if (right_way_up==false) {
Fn = 0.0f;
Ft = 0.0f;
wheel->forces.x = 0.0f;
wheel->forces.y = 0.0f;
wheel->forces.z = 0.0f;
Ft2 = 0.0f;
wheel->spinTq = 0.0f;
} else {
BEGIN_PROFILE (timer_wheel_to_car);
t3Dd f;
// send friction and reaction forces to the car
// normally we would not need to add f.z here, as that
// would be purely coming from the suspension. However
// that would only be the case if the wheels were really
// independent objects. Right now their position is determined
// in update ride, so we have no choice but to transmit the
// suspension-parallel friction forces magically to the car.
f.x = wheel->forces.x;
f.y = wheel->forces.y;
f.z = wheel->forces.z;
// TODO: Check whether this is correct.
angles.x = wheel->relPos.ax + asin(rel_normal.x);
angles.y = asin(rel_normal.y);
angles.z = waz;
NaiveInverseRotate (f, angles, &wheel->forces);
// transmit reaction forces to the car
wheel->forces.x +=(Ft* CosA - Fn * SinA);
wheel->forces.y +=(Ft* SinA + Fn * CosA);
//RELAXATION2(wheel->forces.x, wheel->preFn, 50.0f);
//RELAXATION2(wheel->forces.y, wheel->preFt, 50.0f);
wheel->forces.z = f_z + wheel->bump_force; // only suspension acts on z axis.
car->carElt->_wheelFx(index) = wheel->forces.x;
car->carElt->_wheelFz(index) = wheel->forces.z;
wheel->spinTq = (Ft2 + tanh(wrl)*fabs(wheel->rollRes))* adjRadius;
wheel->sa = sa;
wheel->sx = sx;
END_PROFILE (timer_wheel_to_car);
}
wheel->feedBack.spinVel = wheel->spinVel;
wheel->feedBack.Tq = wheel->spinTq;
wheel->feedBack.brkTq = wheel->brake.Tq;
car->carElt->_tyreT_mid(index) = wheel->T_current;
car->carElt->_tyreCondition(index) = wheel->condition;
car->carElt->_wheelSlipSide(index) = wvy;
car->carElt->_wheelSlipAccel(index) = wvx;
}
//
// todo: this is random access, it can be walked 'cache friendly'!
//
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
{
/*if (0)
{
int maxNumManifolds = dispatcher->getNumManifolds();
btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher;
btPersistentManifold** manifold = colDis->getInternalManifoldPointer();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0);
return;
}
*/
BEGIN_PROFILE("islandUnionFindAndHeapSort");
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
getUnionFind().sortIslands();
int numElem = getUnionFind().getNumElements();
int endIslandIndex=1;
int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
printf("error in island management\n");
}
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
printf("error in island management\n");
}
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
colObj0->setActivationState( ISLAND_SLEEPING );
}
}
} else
{
int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
printf("error in island management\n");
}
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if ( colObj0->getActivationState() == ISLAND_SLEEPING)
{
colObj0->setActivationState( WANTS_DEACTIVATION);
}
}
}
}
}
btAlignedObjectArray<btPersistentManifold*> islandmanifold;
int i;
int maxNumManifolds = dispatcher->getNumManifolds();
islandmanifold.reserve(maxNumManifolds);
for (i=0;i<maxNumManifolds ;i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
//todo: check sleeping conditions!
if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObj0->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
{
colObj1->activate();
}
if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
{
colObj0->activate();
}
//filtering for response
if (dispatcher->needsResponse(colObj0,colObj1))
islandmanifold.push_back(manifold);
}
}
int numManifolds = int (islandmanifold.size());
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
islandmanifold.heapSort(btPersistentManifoldSortPredicate());
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//int islandId;
END_PROFILE("islandUnionFindAndHeapSort");
btAlignedObjectArray<btCollisionObject*> islandBodies;
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
islandBodies.push_back(colObj0);
if (!colObj0->isActive())
islandSleeping = true;
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
if (startManifoldIndex<numManifolds)
{
int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = &islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
}
if (!islandSleeping)
{
callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId);
}
if (numIslandManifolds)
{
startManifoldIndex = endManifoldIndex;
}
islandBodies.resize(0);
}
}
