void quaternion_t::compute_rotation(const vector_t& v1, const vector_t& v2)
{
PRX_ASSERT(v1.get_dim() == v2.get_dim());
vector_t v(3);
double w;
v.cross_product(v1,v2);
w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
set(v[0],v[1],v[2],w);
normalize();
}
bool geometry_t::contains_point(const vector_t& v, double buffer)
{
double squared_rad;
vector_t n;
switch(type)
{
case PRX_SPHERE://sphere
squared_rad = info[0]*info[0];
if(v.squared_norm() < squared_rad+buffer)
return true;
break;
case PRX_BOX://box
if(v[0]>-.5*info[0]-buffer && v[0]<.5*info[0]+buffer &&
v[1]>-.5*info[1]-buffer && v[1]<.5*info[1]+buffer &&
v[2]>-.5*info[2]-buffer && v[2]<.5*info[2]+buffer)
{
return true;
}
break;
case PRX_CONE:
PRX_WARN_S("Cannot check point containment in cone");
break;
case PRX_CYLINDER://cylinder
case PRX_OPEN_CYLINDER:
n = v;
n[2] = 0;
squared_rad = info[0]*info[0]+buffer;
if(v.squared_norm() < squared_rad &&
v[2]>-.5*info[1]-buffer && v[2]<.5*info[1]+buffer )
return true;
break;
case PRX_CAPSULE:
PRX_WARN_S("Cannot check point containment in capsule");
break;
case PRX_MESH:
// PRX_WARN_S(trimesh.min_x<<" "<<trimesh.max_x<<" "<<trimesh.min_y<<" "<<trimesh.max_y<<" "<<trimesh.min_z<<" "<<trimesh.max_z);
if( trimesh.min_x - buffer < v[0] && trimesh.max_x + buffer > v[0]
&& trimesh.min_y - buffer < v[1] && trimesh.max_y + buffer > v[1]
&& trimesh.min_z - buffer < v[2] && trimesh.max_z + buffer > v[2])
return true;
break;
default:
PRX_WARN_S("Cannot check point containment in geometry");
}
return false;
}
void quaternion_t::compute_rotation(const vector_t& v1, const vector_t& v2)
{
PRX_ASSERT(v1.get_dim() == v2.get_dim());
vector_t v(3);
double w;
// v.cross_product(v1,v2);
//
// if ( fabs(v1.norm()-1) <= PRX_ZERO_CHECK && fabs(v2.norm()-1) <= PRX_ZERO_CHECK ) {
// w = 1 + v1.dot_product(v2);
// } else {
// w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
// }
//
// set(v[0],v[1],v[2],w);
v.cross_product(v1,v2);
w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
set(v[0],v[1],v[2],w);
// PRX_DEBUG_S("q: " << *this);
normalize();
// PRX_DEBUG_S("AFTER q: " << *this);
}
