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