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); }