/// Converts a rotation matrix to a unit Quaternion
Quatd to_Quat(const Matrix3d& m)
{
const unsigned X = 0, Y = 1, Z = 2;
// core computation
Quatd q;
q.w = std::sqrt(std::max(0.0, 1.0 + m(X,X) + m(Y,Y) + m(Z,Z))) * 0.5;
q.x = std::sqrt(std::max(0.0, 1.0 + m(X,X) - m(Y,Y) - m(Z,Z))) * 0.5;
q.y = std::sqrt(std::max(0.0, 1.0 - m(X,X) + m(Y,Y) - m(Z,Z))) * 0.5;
q.z = std::sqrt(std::max(0.0, 1.0 - m(X,X) - m(Y,Y) + m(Z,Z))) * 0.5;
// sign computation
if (m(Z,Y) - m(Y,Z) < 0.0)
q.x = -q.x;
if (m(X,Z) - m(Z,X) < 0.0)
q.y = -q.y;
if (m(Y,X) - m(X,Y) < 0.0)
q.z = -q.z;
#ifndef NEXCEPT
if (!q.unit())
std::cerr << "Quatd::set() warning! not a unit quaternion!" << std::endl;
#endif
return q;
}
bool Model::getBonePosInt(const char *boneName, const MotionPose &v0, const Bone *bone, const Vec3d &spos, const Quatd &srot, Vec3d *pos, Quatd *rot)const{
Vec3d apos = spos;
Quatd arot = srot;
for(const MotionPose *v = &v0; v; v = v->next){
MotionPose::const_iterator it = v->nodes.find(bone->name);
// for(int i = 0; i < bones; i++) if(!strcmp(bonevar[i].name, bone->name)){
if(it != v->nodes.end()){
apos += arot.trans(bone->joint);
apos += arot.trans(it->second.pos);
arot *= it->second.rot;
apos -= arot.trans(bone->joint);
}
}
if(!strcmp(boneName, bone->name)){
if(pos)
*pos = apos + arot.trans(bone->joint);
if(rot)
*rot = arot;
return true;
}
for(const Bone *nextbone = bone->children; nextbone; nextbone = nextbone->nextSibling){
if(nextbone->suf){
if(getBonePosInt(boneName, v0, nextbone, apos, arot, pos, rot))
return true;
}
}
return false;
}
//------------------------------------------------------------------------------
//!
void
CameraManipulator::tilt( float primAngle, float secAngle )
{
primAngle *= _flipTiltH;
secAngle *= _flipTiltV;
// TODO
// Clamping of secondary angle...
// Rotate the referential.
camera()->referential( _ref.getRotated( _primaryAxis, primAngle ) );
// Calculate the rotation quaternion and it matrix.
Quatd quat = Quatd::axisAngle( _primaryAxis, primAngle );
Mat4d mat = quat.toMatrix();
// Rotate the SecondaryAxis.
Vec3f secondaryAxis = mat * _secondaryAxis;
// Rotate the referential.
Reff ref = camera()->referential().getRotated( secondaryAxis, secAngle );
ref.orientation().normalize();
camera()->referential( ref );
// Move the POI.
_poi = mat * ( _grabPoi - _ref.position() );
quat = Quatd::axisAngle( secondaryAxis, secAngle );
_poi = quat.toMatrix()*_poi;
_poi += _ref.position();
}
bool Model::getBonePosInt(const char *boneName, const ysdnmv_t &v0, const Bone *bone, const Vec3d &spos, const Quatd &srot, Vec3d *pos, Quatd *rot)const{
Vec3d apos = spos;
Quatd arot = srot;
for(const ysdnmv_t *v = &v0; v; v = v->next){
ysdnm_bone_var *bonevar = v->bonevar;
int bones = min(v->bones, this->n);
for(int i = 0; i < bones; i++) if(!strcmp(bonevar[i].name, bone->name)){
apos += arot.trans(bone->joint);
apos += arot.trans(v->bonevar[i].pos);
arot *= bonevar[i].rot;
apos -= arot.trans(bone->joint);
}
}
if(!strcmp(boneName, bone->name)){
if(pos)
*pos = apos + arot.trans(bone->joint);
if(rot)
*rot = arot;
return true;
}
for(const Bone *nextbone = bone->children; nextbone; nextbone = nextbone->nextSibling){
if(nextbone->suf){
if(getBonePosInt(boneName, v0, nextbone, apos, arot, pos, rot))
return true;
}
}
return false;
}
/// turn to face a given world-coordinate point
void faceToward(const Vec3d& p, double amt=1.) {
Vec3d rotEuler;
Vec3d target(p - pos());
target.normalize();
Quatd rot = Quatd::getRotationTo(uf(), target);
rot.toEuler(rotEuler);
mTurn.set(rotEuler * amt);
}
void RestShapeSpringsForceField<Rigid3Types>::addForce(const core::MechanicalParams* /* mparams */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& /* v */)
{
sofa::helper::WriteAccessor< DataVecDeriv > f1 = f;
sofa::helper::ReadAccessor< DataVecCoord > p1 = x;
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
f1.resize(p1.size());
if (recompute_indices.getValue())
{
recomputeIndices();
}
const VecReal& k = stiffness.getValue();
const VecReal& k_a = angularStiffness.getValue();
for (unsigned int i = 0; i < m_indices.size(); i++)
{
const unsigned int index = m_indices[i];
unsigned int ext_index = m_indices[i];
if(useRestMState)
ext_index= m_ext_indices[i];
// translation
if (i >= m_pivots.size())
{
Vec3d dx = p1[index].getCenter() - p0[ext_index].getCenter();
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
}
else
{
CPos localPivot = p0[ext_index].getOrientation().inverseRotate(m_pivots[i] - p0[ext_index].getCenter());
CPos rotatedPivot = p1[index].getOrientation().rotate(localPivot);
CPos pivot2 = p1[index].getCenter() + rotatedPivot;
CPos dx = pivot2 - m_pivots[i];
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
}
// rotation
Quatd dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse();
Vec3d dir;
double angle=0;
dq.normalize();
if (dq[3] < 0)
{
dq = dq * -1.0;
}
if (dq[3] < 0.999999999999999)
dq.quatToAxis(dir, angle);
getVOrientation(f1[index]) -= dir * angle * (i < k_a.size() ? k_a[i] : k_a[0]);
}
}
Matrix3d to_Matrix3d(const Quatd& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// verify that the quaternion is normalized
assert(std::fabs(q.magnitude()) - 1 < EPS_FLOAT);
// setup repeated products
const double xx = q.x*q.x;
const double xy = q.x*q.y;
const double xz = q.x*q.z;
const double xw = q.x*q.w;
const double yy = q.y*q.y;
const double yz = q.y*q.z;
const double yw = q.y*q.w;
const double zz = q.z*q.z;
const double zw = q.z*q.w;
const double ww = q.w*q.w;
Matrix3d m;
m(X,X) = 2*(xx + ww) - 1;
m(X,Y) = 2*(xy - zw);
m(X,Z) = 2*(xz + yw);
m(Y,X) = 2*(xy + zw);
m(Y,Y) = 2*(yy + ww) - 1;
m(Y,Z) = 2*(yz - xw);
m(Z,X) = 2*(xz - yw);
m(Z,Y) = 2*(yz + xw);
m(Z,Z) = 2*(zz + ww) - 1;
return m;
}
/// Sets quaternion to that represented by an axis-angle representation
Quatd to_Quat(const AAngled& a)
{
const double half = a.angle*0.5;
double sina = std::sin(half);
Quatd q;
q.x = a.x * sina;
q.y = a.y * sina;
q.z = a.z * sina;
q.w = std::cos(half);
#ifndef NEXCEPT
if (!q.unit())
std::cerr << "Quatd::set() warning! not a unit quaternion!" << std::endl;
#endif
return q;
}
//------------------------------------------------------------------------------
//!
void
CameraManipulator::orbit( float primAngle, float secAngle )
{
primAngle *= _flipOrbitH;
secAngle *= _flipOrbitV;
// Rotate the referential.
camera()->referential( _ref.getRotated( _poi, _primaryAxis, primAngle ) );
// Rotate the SecondaryAxis.
Quatd quat = Quatd::axisAngle( _primaryAxis, primAngle );
Vec3f secondaryAxis = quat.toMatrix() * _secondaryAxis;
// Rotate the referential.
Reff ref = camera()->referential().getRotated( _poi, secondaryAxis, secAngle );
ref.orientation().normalize();
camera()->referential( ref );
}
//--------------------------------------------------------------------------------------------------
/// Rotate
///
/// \return Returns true if input caused changes to the camera an false if no changes occurred
//--------------------------------------------------------------------------------------------------
bool ManipulatorTrackball::rotate(int posX, int posY)
{
if (m_camera.isNull()) return false;
if (posX == m_lastPosX && posY == m_lastPosY) return false;
const double vpPixSizeX = m_camera->viewport()->width();
const double vpPixSizeY = m_camera->viewport()->height();
if (vpPixSizeX <= 0 || vpPixSizeY <= 0) return false;
const double vpPosX = posX - static_cast<int>(m_camera->viewport()->x());
const double vpPosY = posY - static_cast<int>(m_camera->viewport()->y());
const double vpLastPosX = m_lastPosX - static_cast<int>(m_camera->viewport()->x());
const double vpLastPosY = m_lastPosY - static_cast<int>(m_camera->viewport()->y());
// Scale the new/last positions to the range [-1.0, 1.0]
double newPosX = 2.0*(vpPosX/vpPixSizeX) - 1.0;
double newPosY = 2.0*(vpPosY/vpPixSizeY) - 1.0;
double lastPosX = 2.0*(vpLastPosX/vpPixSizeX) - 1.0;
double lastPosY = 2.0*(vpLastPosY/vpPixSizeY) - 1.0;
Mat4d viewMat = m_camera->viewMatrix();
// Compute rotation quaternion
Quatd rotQuat = trackballRotation(lastPosX, lastPosY, newPosX, newPosY, viewMat, m_rotateSensitivity);
// Update navigation by modifying the view matrix
Mat4d rotMatr = rotQuat.toMatrix4();
rotMatr.translatePostMultiply(-m_rotationPoint);
rotMatr.translatePreMultiply(m_rotationPoint);
viewMat = viewMat*rotMatr;
m_camera->setViewMatrix(viewMat);
m_lastPosX = posX;
m_lastPosY = posY;
return true;
}
static void longLatLabel(const string& labelText,
double longitude,
double latitude,
const Vec3d& viewRayOrigin,
const Vec3d& viewNormal,
const Vec3d& bodyCenter,
const Quatd& bodyOrientation,
const Vec3f& semiAxes,
float labelOffset,
Renderer* renderer)
{
double theta = degToRad(longitude);
double phi = degToRad(latitude);
Vec3d pos(cos(phi) * cos(theta) * semiAxes.x,
sin(phi) * semiAxes.y,
-cos(phi) * sin(theta) * semiAxes.z);
float nearDist = renderer->getNearPlaneDistance();
pos = pos * (1.0 + labelOffset);
double boundingRadius = max(semiAxes.x, max(semiAxes.y, semiAxes.z));
// Draw the label only if it isn't obscured by the body ellipsoid
double t = 0.0;
if (testIntersection(Ray3d(Point3d(0.0, 0.0, 0.0) + viewRayOrigin, pos - viewRayOrigin),
Ellipsoidd(Vec3d(semiAxes.x, semiAxes.y, semiAxes.z)), t) && t >= 1.0)
{
// Compute the position of the label
Vec3d labelPos = bodyCenter +
(1.0 + labelOffset) * pos * bodyOrientation.toMatrix3();
// Calculate the intersection of the eye-to-label ray with the plane perpendicular to
// the view normal that touches the front of the objects bounding sphere
double planetZ = viewNormal * bodyCenter - boundingRadius;
if (planetZ < -nearDist * 1.001)
planetZ = -nearDist * 1.001;
double z = viewNormal * labelPos;
labelPos *= planetZ / z;
renderer->addObjectAnnotation(NULL, labelText,
Renderer::PlanetographicGridLabelColor,
Point3f((float) labelPos.x, (float) labelPos.y, (float) labelPos.z));
}
}
void turn(const Quatd& v) {
v.toEuler(mSpin1);
}
Point3d SynchronousOrbit::positionAtTime(double jd) const
{
Quatd q = body.getEquatorialToBodyFixed(jd);
return position * q.toMatrix3();
}
void UniformMass<RigidTypes, MassType>::loadFromFileRigidImpl(const string& filename)
{
TemporaryLocale locale(LC_ALL, "C") ;
if (!filename.empty())
{
MassType m = getMass();
string unconstingFilenameQuirck = filename ;
if (!DataRepository.findFile(unconstingFilenameQuirck))
msg_error(this) << "cannot find file '" << filename << "'.\n" ;
else
{
char cmd[64];
FILE* file;
if ((file = fopen(filename.c_str(), "r")) == NULL)
{
msg_error(this) << "cannot open file '" << filename << "'.\n" ;
}
else
{
{
skipToEOL(file);
ostringstream cmdScanFormat;
cmdScanFormat << "%" << (sizeof(cmd) - 1) << "s";
while (fscanf(file, cmdScanFormat.str().c_str(), cmd) != EOF)
{
if (!strcmp(cmd,"inrt"))
{
for (int i = 0; i < 3; i++){
for (int j = 0; j < 3; j++){
double tmp = 0;
if( fscanf(file, "%lf", &(tmp)) < 1 ){
msg_error(this) << "error while reading file '" << filename << "'.\n";
}
m.inertiaMatrix[i][j]=tmp;
}
}
}
else if (!strcmp(cmd,"cntr") || !strcmp(cmd,"center") )
{
Vec3d center;
for (int i = 0; i < 3; ++i)
{
if( fscanf(file, "%lf", &(center[i])) < 1 ){
msg_error(this) << "error reading file '" << filename << "'.";
}
}
}
else if (!strcmp(cmd,"mass"))
{
double mass;
if( fscanf(file, "%lf", &mass) > 0 )
{
if (!this->d_mass.isSet())
m.mass = mass;
}
else
msg_error(this) << "error reading file '" << filename << "'." << msgendl
<< "Unable to decode command 'mass'";
}
else if (!strcmp(cmd,"volm"))
{
double tmp;
if( fscanf(file, "%lf", &(tmp)) < 1 )
msg_error(this) << "error reading file '" << filename << "'." << msgendl
<< "Unable to decode command 'volm'." << msgendl;
m.volume = tmp ;
}
else if (!strcmp(cmd,"frme"))
{
Quatd orient;
for (int i = 0; i < 4; ++i)
{
if( fscanf(file, "%lf", &(orient[i])) < 1 )
msg_error(this) << "error reading file '" << filename << "'." << msgendl
<< "Unable to decode command 'frme' at index " << i ;
}
orient.normalize();
}
else if (!strcmp(cmd,"grav"))
{
Vec3d gravity;
if( fscanf(file, "%lf %lf %lf\n", &(gravity.x()), &(gravity.y()), &(gravity.z())) < 3 )
msg_warning(this) << "error reading file '" << filename << "'." << msgendl
<< " Unable to decode command 'gravity'.";
}
else if (!strcmp(cmd,"visc"))
{
double viscosity = 0;
if( fscanf(file, "%lf", &viscosity) < 1 )
msg_warning(this) << "error reading file '" << filename << "'.\n"
" Unable to decode command 'visc'. \n";
}
else if (!strcmp(cmd,"stck"))
{
double tmp;
if( fscanf(file, "%lf", &tmp) < 1 ) //&(MSparams.default_stick));
msg_warning(this) << "error reading file '" << filename << "'.\n"
<< "Unable to decode command 'stck'. \n";
}
else if (!strcmp(cmd,"step"))
{
double tmp;
if( fscanf(file, "%lf", &tmp) < 1 ) //&(MSparams.default_dt));
msg_warning(this) << "error reading file '" << filename << "'.\n"
<< "Unable to decode command 'step'. \n";
}
else if (!strcmp(cmd,"prec"))
{
double tmp;
if( fscanf(file, "%lf", &tmp) < 1 ) //&(MSparams.default_prec));
{
msg_warning(this) << "error reading file '" << filename << "'.\n"
<< "Unable to decode command 'prec'. \n" ;
}
}
else if (cmd[0] == '#') // it's a comment
{
skipToEOL(file);
}
else // it's an unknown keyword
{
msg_warning(this) << "error reading file '" << filename << "'. \n"
<< "Unable to decode an unknow command '"<< cmd << "'. \n" ;
skipToEOL(file);
}
}
}
fclose(file);
}
}
setMass(m);
}
else if (d_totalMass.getValue()>0 && mstate!=NULL) d_mass.setValue((Real)d_totalMass.getValue() / mstate->getSize());
}
Quatd frame_rot() {
Quatd rot;
rot.clear();
rot = rotation(M_PI * 0.5, make_vector< double >(1,0,0));
return rot;
}
TEST(Boxes, ConstraintViolation)
{
const double TOL = 1e-6;
const double DT = 1e-2;
const std::string FNAME("box.xml");
const std::string BOX_ID("box");
shared_ptr<TimeSteppingSimulator> sim;
shared_ptr<RigidBody> box;
const unsigned NTIMES = 100;
// log contact
Moby::Log<Moby::OutputToFile>::reporting_level = (LOG_SIMULATOR | LOG_CONSTRAINT);
Moby::OutputToFile::stream.open("logging.out");
// log energy
std::ofstream energy_out("energy.dat");
// do this multiple times
for (unsigned j=0; j< NTIMES; j++)
{
// load in the box file
map<std::string, BasePtr> READ_MAP = XMLReader::read(FNAME);
// look for the simulator
find(READ_MAP, sim);
// look for the box
find(READ_MAP, box, BOX_ID);
// set tolerance for constraint stabilization and minimum step size
// sim->min_step_size = TOL/10000.0;
// sim->min_step_size = 1e-5;
sim->min_step_size = 1e-1;
sim->cstab.eps = -NEAR_ZERO;
// modify the initial orientation of the box
Quatd q;
q.x = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.y = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.z = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.w = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.normalize();
Pose3d P = *box->get_pose();
P.q = q;
box->set_pose(P);
// modify the velocity of the box
Vector3d xd(box->get_inertial_pose());
Vector3d w(box->get_inertial_pose());
SVelocityd v(box->get_inertial_pose());
for (unsigned i=0; i< 3; i++)
{
xd[i] = (double) rand() / RAND_MAX * 2.0 - 1.0;
w[i] = (double) rand() / RAND_MAX * 2.0 - 1.0;
}
v.set_linear(xd);
v.set_angular(w);
box->set_velocity(v);
// set the first no K.E. time
double no_KE_time = 0.0;
// setup the maximum violation
double max_vio = 0.0;
// reset steps
unsigned step = 0;
// integrate the box until it has no kinetic energy for 1/2 sec
while (true)
{
// execute the step
sim->step(DT);
// get the constraint violation
const vector<PairwiseDistInfo>& pdi = sim->get_pairwise_distances();
for (unsigned i=0; i< pdi.size(); i++)
max_vio = std::min(max_vio, pdi[i].dist);
// get the kinetic energy of the box
double KE = box->calc_kinetic_energy();
energy_out << KE << std::endl;
// see whether there is no kinetic energy
if (KE < 1e-6)
{
if (sim->current_time - no_KE_time > 0.5)
break;
}
else
no_KE_time = sim->current_time;
if (step++ % 1000 == 0)
std::cerr << "." << std::flush;
}
// only want to print out one message about violation
EXPECT_GT(max_vio, -TOL);
std::cerr << "+" << std::flush;
energy_out.close();
}
}
void WarSpace::drawtra(wardraw_t *wd){
Teline3DrawData dd;
dd.viewdir = -wd->vw->rot.vec3(2);
dd.viewpoint = wd->vw->pos;
dd.invrot = wd->vw->irot;
dd.fov = wd->vw->fov;
dd.pgc = wd->vw->gc;
dd.rot = wd->vw->qrot;
dd.user = this;
DrawTeline3D(tell, &dd);
tepl->draw(wd->vw->pos, &static_cast<glcull>(*wd->vw->gc));
for(int i = 0; i < 2; i++)
for(WarField::EntityList::iterator it = (this->*list[i]).begin(); it != (this->*list[i]).end(); it++){
if(!*it)
continue;
Entity *pe = *it;
if(pe->w == this/* && wd->vw->zslice == (pl->chase && pl->mover == &Player::freelook && pl->chase->getUltimateOwner() == pe->getUltimateOwner() ? 0 : 1)*/){
try{
pe->drawtra(wd);
}
catch(std::exception e){
fprintf(stderr, __FILE__"(%d) Exception in %p->%s::drawtra(): %s\n", __LINE__, pe, pe->idname(), e.what());
}
catch(...){
fprintf(stderr, __FILE__"(%d) Exception in %p->%s::drawtra(): ?\n", __LINE__, pe, pe->idname());
}
}
}
if(g_debugdraw_bullet && bdw){
static class DebugDraw : public btIDebugDraw{
// Override only drawLine method for the moment.
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color){
glColor3fv(btvc(color).cast<float>());
glBegin(GL_LINES);
glVertex3dv(btvc(from));
glVertex3dv(btvc(to));
glEnd();
}
virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color){}
virtual void reportErrorWarning(const char* warningString){}
virtual void draw3dText(const btVector3& location,const char* textString){}
virtual void setDebugMode(int debugMode){}
virtual int getDebugMode() const{return 1;}
}debugDrawer;
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glDepthMask(GL_FALSE);
glEnable(GL_BLEND);
bdw->getCollisionWorld()->setDebugDrawer(&debugDrawer);
bdw->debugDrawWorld();
}
if(g_player_viewport){
Game *game = wd->w->getGame();
for(Game::PlayerList::iterator it = game->players.begin(); it != game->players.end(); ++it){
Player *player = *it;
if(player == game->player || player->cs != cs)
continue;
double f = player->fov;
const Vec3d pos = player->getpos();
const Quatd irot = player->getrot().cnj();
glBegin(GL_LINES);
for(int level = 4; 0 <= level; --level){
double l = double(1 << level);
glColor4f(!!((1 << level) & 8), !!((1 << level) & 5), !!((1 << level) & 19), 1);
glVertex3dv(pos);
glVertex3dv(pos + irot.trans(l * Vec3d(0,0,-1)));
for(int y = -1; y <= 1; y++){
glVertex3dv(pos + irot.trans(l * Vec3d(-f, y * f, -1)));
glVertex3dv(pos + irot.trans(l * Vec3d(f, y * f, -1)));
}
for(int x = -1; x <= 1; x++){
glVertex3dv(pos + irot.trans(l * Vec3d(x * f, -f,-1)));
glVertex3dv(pos + irot.trans(l * Vec3d(x * f, f,-1)));
}
glVertex3dv(pos);
glVertex3dv(pos + irot.trans(l * Vec3d(-1,-1,-1)));
glVertex3dv(pos);
glVertex3dv(pos + irot.trans(l * Vec3d(1,-1,-1)));
glVertex3dv(pos);
glVertex3dv(pos + irot.trans(l * Vec3d(-1,1,-1)));
glVertex3dv(pos);
glVertex3dv(pos + irot.trans(l * Vec3d(1,1,-1)));
}
glEnd();
}
}
if(g_otdrawflags)
ot_draw(this, wd);
}
