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(); }
void quaternion_t::get( vector_t& v) const { PRX_ASSERT(v.get_dim() == 4); v[0] = q[0]; v[1] = q[1]; v[2] = q[2]; v[3] = q[3]; }
void quaternion_t::set( const vector_t& v) { PRX_ASSERT(v.get_dim() == 4); q[0] = v[0]; q[1] = v[1]; q[2] = v[2]; q[3] = v[3]; }
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); }
void quaternion_t::set( const vector_t& axis, double angle) { PRX_ASSERT(axis.get_dim() == 3); if (fabs(angle) < PRX_ZERO_CHECK) { clear(); return; } angle *= 0.5; double sinAngle = sin(angle); q[0] = axis[0] * sinAngle; q[1] = axis[1] * sinAngle; q[2] = axis[2] * sinAngle; q[3] = cos(angle); }
vector_t::vector_t( const vector_t & other ) { _vec.resize( other.get_dim() ); _vec = other._vec; }
void geometry_t::init(const parameter_reader_t* const reader) { const std::string geom_type = reader->get_attribute("type"); // PRX_DEBUG_COLOR("MAKING GEOMETRY " << geom_type.c_str(), PRX_TEXT_CYAN); if (geom_type == "box") { const vector_t dims = reader->get_attribute_as<vector_t>("dims"); if (dims.get_dim() != 3) PRX_FATAL_S("Box must have three-dimensional dims attribute.") set_box(dims[0], dims[1], dims[2]); } else if (geom_type == "sphere") set_sphere(reader->get_attribute_as<double>("radius")); else if (geom_type == "cone") set_cone(reader->get_attribute_as<double>("radius"), reader->get_attribute_as<double>("height")); else if (geom_type == "cylinder") set_cylinder(reader->get_attribute_as<double>("radius"), reader->get_attribute_as<double>("height")); else if (geom_type == "open_cylinder") set_open_cylinder(reader->get_attribute_as<double>("radius"), reader->get_attribute_as<double>("height")); else if (geom_type == "capsule") set_capsule(reader->get_attribute_as<double>("radius"), reader->get_attribute_as<double>("height")); else if (geom_type == "mesh") set_mesh(reader->get_attribute_as<std::string>("filename")); else if (geom_type == "point_cloud") set_point_cloud(); else if (geom_type == "heightmap") set_heightmap(reader->get_attribute_as<std::string>("filename")); else if (geom_type == "polygon") { const std::vector< double > triangles = reader->get_attribute_as< std::vector<double> >("triangles"); set_polygon( triangles ); } else { const std::string trace = reader->trace(); PRX_FATAL_S("Unrecognized geometry type (" << geom_type.c_str() << ") in " << trace.c_str()); } if ( reader->has_attribute("scale")) { scale = reader->get_attribute_as<vector_t>("scale"); if (scale.get_dim() != 3) PRX_FATAL_S("Scale must have 3 elements at " << reader->trace() ); } else set_scale(1.0,1.0,1.0); if( reader->has_attribute("material" ) ) { std::string color_string = reader->get_attribute("material"); if( color_string == "yellow" ) { color[0] = 1; color[1] = 1; color[2] = 0; color[3] = 1.0; } else if( color_string == "blue" ) { color[0] = 0; color[1] = 0; color[2] = 1; color[3] = 1.0; } else if( color_string == "dark_grey" ) { color[0] = 0.25; color[1] = 0.25; color[2] = 0.25; color[3] = 1.0; } else if( color_string == "black" ) { color[0] = 0; color[1] = 0; color[2] = 0; color[3] = 1.0; } else if( color_string == "green" ) { color[0] = 0; color[1] = 1; color[2] = 0; color[3] = 1.0; } else if( color_string == "red" ) { color[0] = 1; color[1] = 0; color[2] = 0; color[3] = 1.0; } else if( color_string == "silver" ) { color[0] = 0.75; color[1] = 0.75; color[2] = 0.75; color[3] = 1.0; } else if( color_string == "cyan" ) { color[0] = 0.7; color[1] = 1; color[2] = 1; color[3] = 1.0; } else if( color_string == "orange" ) { color[0] = 1; color[1] = 0.6; color[2] = 0.05; color[3] = 1.0; } else if( color_string == "brown" ) { color[0] = 0.4; color[1] = 0.25; color[2] = 0.0; color[3] = 1.0;} else if( color_string == "glass" ) { color[0] = 0.5; color[1] = 0.5; color[2] = 0.55; color[3] = 0.18;} else { color[0] = 1; color[1] = 1; color[2] = 1; color[3] = 1.0; } } else { color[0] = 1; color[1] = 1; color[2] = 1; color[3] = 1.0; } }
void config_t::set_position(const vector_t& pos) { PRX_ASSERT(pos.get_dim() == 3); position.copy(pos); }