/// @brief OBB merge method when the centers of two smaller OBB are close
inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
{
OBB b;
b.To = (b1.To + b2.To) * 0.5;
Quaternion3f q0, q1;
q0.fromAxes(b1.axis);
q1.fromAxes(b2.axis);
if(q0.dot(q1) < 0)
q1 = -q1;
Quaternion3f q = q0 + q1;
FCL_REAL inv_length = 1.0 / std::sqrt(q.dot(q));
q = q * inv_length;
q.toAxes(b.axis);
Vec3f vertex[8], diff;
FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
Vec3f pmin(real_max, real_max, real_max);
Vec3f pmax(-real_max, -real_max, -real_max);
computeVertices(b1, vertex);
for(int i = 0; i < 8; ++i)
{
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
FCL_REAL dot = diff.dot(b.axis[j]);
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
pmin[j] = dot;
}
}
computeVertices(b2, vertex);
for(int i = 0; i < 8; ++i)
{
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
FCL_REAL dot = diff.dot(b.axis[j]);
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
pmin[j] = dot;
}
}
for(int j = 0; j < 3; ++j)
{
b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
}
return b;
}
void InterpMotion::computeVelocity()
{
linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
deltaq.toAxisAngle(angular_axis, angular_vel);
if(angular_vel < 0)
{
angular_vel = -angular_vel;
angular_axis = -angular_axis;
}
}
Transform3f RevoluteJoint::getLocalTransform(const JointConfig& cfg) const
{
Quaternion3f quat;
quat.fromAxisAngle(axis_, cfg[0]);
return Transform3f(transform_to_parent_.getQuatRotation() * quat, transform_to_parent_.getTranslation());
}
void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh)
{
std::vector<TStruct> ts;
std::vector<Timer> timers;
std::vector<CollisionObject*> env;
if(use_mesh)
generateEnvironmentsMesh(env, env_scale, env_size);
else
generateEnvironments(env, env_scale, env_size);
std::vector<CollisionObject*> query;
if(use_mesh)
generateEnvironmentsMesh(query, env_scale, query_size);
else
generateEnvironments(query, env_scale, query_size);
std::vector<BroadPhaseCollisionManager*> managers;
managers.push_back(new NaiveCollisionManager());
managers.push_back(new SSaPCollisionManager());
managers.push_back(new SaPCollisionManager());
managers.push_back(new IntervalTreeCollisionManager());
Vec3f lower_limit, upper_limit;
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20);
// managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit));
managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit));
#if USE_GOOGLEHASH
managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
#endif
managers.push_back(new DynamicAABBTreeCollisionManager());
managers.push_back(new DynamicAABBTreeCollisionManager_Array());
{
DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
m->tree_init_level = 2;
managers.push_back(m);
}
{
DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array();
m->tree_init_level = 2;
managers.push_back(m);
}
ts.resize(managers.size());
timers.resize(managers.size());
for(size_t i = 0; i < managers.size(); ++i)
{
timers[i].start();
managers[i]->registerObjects(env);
timers[i].stop();
ts[i].push_back(timers[i].getElapsedTime());
}
for(size_t i = 0; i < managers.size(); ++i)
{
timers[i].start();
managers[i]->setup();
timers[i].stop();
ts[i].push_back(timers[i].getElapsedTime());
}
// update the environment
FCL_REAL delta_angle_max = 10 / 360.0 * 2 * boost::math::constants::pi<FCL_REAL>();
FCL_REAL delta_trans_max = 0.01 * env_scale;
for(size_t i = 0; i < env.size(); ++i)
{
FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
Quaternion3f q1, q2, q3;
q1.fromAxisAngle(Vec3f(1, 0, 0), rand_angle_x);
q2.fromAxisAngle(Vec3f(0, 1, 0), rand_angle_y);
q3.fromAxisAngle(Vec3f(0, 0, 1), rand_angle_z);
Quaternion3f q = q1 * q2 * q3;
Matrix3f dR;
q.toRotation(dR);
Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
Matrix3f R = env[i]->getRotation();
Vec3f T = env[i]->getTranslation();
env[i]->setTransform(dR * R, dR * T + dT);
env[i]->computeAABB();
}
for(size_t i = 0; i < managers.size(); ++i)
{
timers[i].start();
managers[i]->update();
timers[i].stop();
ts[i].push_back(timers[i].getElapsedTime());
}
std::vector<CollisionData> self_data(managers.size());
for(size_t i = 0; i < managers.size(); ++i)
{
if(exhaustive) self_data[i].request.num_max_contacts = 100000;
else self_data[i].request.num_max_contacts = num_max_contacts;
}
for(size_t i = 0; i < managers.size(); ++i)
{
timers[i].start();
managers[i]->collide(&self_data[i], defaultCollisionFunction);
timers[i].stop();
ts[i].push_back(timers[i].getElapsedTime());
}
for(size_t i = 0; i < managers.size(); ++i)
std::cout << self_data[i].result.numContacts() << " ";
std::cout << std::endl;
if(exhaustive)
{
for(size_t i = 1; i < managers.size(); ++i)
BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts());
}
else
{
std::vector<bool> self_res(managers.size());
for(size_t i = 0; i < self_res.size(); ++i)
self_res[i] = (self_data[i].result.numContacts() > 0);
for(size_t i = 1; i < self_res.size(); ++i)
BOOST_CHECK(self_res[0] == self_res[i]);
for(size_t i = 1; i < managers.size(); ++i)
BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts());
}
for(size_t i = 0; i < query.size(); ++i)
{
std::vector<CollisionData> query_data(managers.size());
for(size_t j = 0; j < query_data.size(); ++j)
{
if(exhaustive) query_data[j].request.num_max_contacts = 100000;
else query_data[j].request.num_max_contacts = num_max_contacts;
}
for(size_t j = 0; j < query_data.size(); ++j)
{
timers[j].start();
managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction);
timers[j].stop();
ts[j].push_back(timers[j].getElapsedTime());
}
// for(size_t j = 0; j < managers.size(); ++j)
// std::cout << query_data[j].result.numContacts() << " ";
// std::cout << std::endl;
if(exhaustive)
{
for(size_t j = 1; j < managers.size(); ++j)
BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts());
}
else
{
std::vector<bool> query_res(managers.size());
for(size_t j = 0; j < query_res.size(); ++j)
query_res[j] = (query_data[j].result.numContacts() > 0);
for(size_t j = 1; j < query_res.size(); ++j)
BOOST_CHECK(query_res[0] == query_res[j]);
for(size_t j = 1; j < managers.size(); ++j)
BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts());
}
}
for(size_t i = 0; i < env.size(); ++i)
delete env[i];
for(size_t i = 0; i < query.size(); ++i)
delete query[i];
for(size_t i = 0; i < managers.size(); ++i)
delete managers[i];
std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
size_t w = 7;
std::cout << "collision timing summary" << std::endl;
std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
std::cout << "register time" << std::endl;
for(size_t i = 0; i < ts.size(); ++i)
std::cout << std::setw(w) << ts[i].records[0] << " ";
std::cout << std::endl;
std::cout << "setup time" << std::endl;
for(size_t i = 0; i < ts.size(); ++i)
std::cout << std::setw(w) << ts[i].records[1] << " ";
std::cout << std::endl;
std::cout << "update time" << std::endl;
for(size_t i = 0; i < ts.size(); ++i)
std::cout << std::setw(w) << ts[i].records[2] << " ";
std::cout << std::endl;
std::cout << "self collision time" << std::endl;
for(size_t i = 0; i < ts.size(); ++i)
std::cout << std::setw(w) << ts[i].records[3] << " ";
std::cout << std::endl;
std::cout << "collision time" << std::endl;
for(size_t i = 0; i < ts.size(); ++i)
{
FCL_REAL tmp = 0;
for(size_t j = 4; j < ts[i].records.size(); ++j)
tmp += ts[i].records[j];
std::cout << std::setw(w) << tmp << " ";
}
std::cout << std::endl;
std::cout << "overall time" << std::endl;
for(size_t i = 0; i < ts.size(); ++i)
std::cout << std::setw(w) << ts[i].overall_time << " ";
std::cout << std::endl;
std::cout << std::endl;
}
Quaternion3f InterpMotion::deltaRotation(FCL_REAL dt) const
{
Quaternion3f res;
res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
return res;
}