int compute_speed(SBullet* bullet, SSimulation simParam, double* scores)
{
int i, j, k;
int idx;
int nObj = bullet->dynamicsWorld->getNumCollisionObjects() - 1;
/// Do some simulation
for (i=0;i<simParam.recordIdx[simParam.nRecordTimes - 1] + 1;i++) {
bullet->dynamicsWorld->stepSimulation(btScalar(simParam.stepSize), 10);
bool f_record = false;
for (k=0; k<simParam.nRecordTimes; k++) {
if (i == simParam.recordIdx[k]) {
f_record = true;
break;
}
}
// Records linear and angular velocities, if necessary
if (f_record) {
for (j=0; j<nObj; j++) {
btCollisionObject* obj =
bullet->dynamicsWorld->getCollisionObjectArray()[j + 1];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState()) {
btVector3 linvel(body->getLinearVelocity());
btVector3 angvel(body->getAngularVelocity());
idx = k * nObj * 2 + j * 2;
scores[idx] = double(linvel.length());
scores[idx + 1] = double(angvel.length());
}
}
}
// //print positions of all objects (good for debugging)
// for (j=0; j<nObj; j++) {
// btCollisionObject* obj =
// bullet->dynamicsWorld->getCollisionObjectArray()[j + 1];
// btRigidBody* body = btRigidBody::upcast(obj);
// if (body && body->getMotionState()) {
// btVector3 linvel(body->getLinearVelocity());
// btVector3 angvel(body->getAngularVelocity());
// // Printout stuff (for debugging)
// if (j >= 0) {
// btTransform trans;
// body->getMotionState()->getWorldTransform(trans);
// printf("\nT %i O %i: ", i, j);
// printf("X %.3f, %.3f, %.3f\t",
// float(trans.getOrigin().getX()),
// float(trans.getOrigin().getY()),
// float(trans.getOrigin().getZ()));
// printf("vL %.3f, %.3f, %.3f |%.3f|\t",
// float(linvel.x()),
// float(linvel.y()),
// float(linvel.z()),
// float(linvel.length()));
// // printf("aV %.3f, %.3f, %.3f |%.3f|\t",
// // float(angvel.x()),
// // float(angvel.y()),
// // float(angvel.z()),
// // float(angvel.length())
// // );
// }
// }
// }
}
return 0;
}
bool KinematicIntegerBands::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship,
const TrafficState& repac, int epsv) const {
// repac is not NULL at this point and k >= 0
if (k==0) {
return true;
}
std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir);
Vect3 so = sovo.first;
Vect3 vo = sovo.second;
Vect3 si = repac.get_s();
Vect3 vi = repac.get_v();
bool rep = true;
if (k==1) {
rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv);
}
if (rep) {
std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir);
Vect3 sot = sovot.first;
Vect3 vot = sovot.second;
Vect3 sit = vi.ScalAdd(k*tstep,si);
Vect3 st = sot.Sub(sit);
Vect3 vop = linvel(ownship,tstep,trajdir,k-1);
Vect3 vok = linvel(ownship,tstep,trajdir,k);
return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) &&
CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) &&
CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv);
}
return false;
}