/*************************************************************************** File: PBMConfiguration.h Created: 03/05/2002 Author: Maxim Garber Computer Science Department University of North Carolina - Chapel Hill garber@cs.unc.edu Description: Configuration for a rigid object ----------------------------------------------------------------------------. Copyright 2002 Maxim Garber UNC Collide Research Group *****************************************************************************/ #include #include #include "PBMConfiguration.h" /**************************************************************************** Function : Config::SetPosition Description: Set position of robot ****************************************************************************/ void Config::SetPosition(float* loc) { for(int i = 0; i < 3; i++) _pPosition[i] = loc[i]; } /**************************************************************************** Function : Config::SetPosition Description: Sets position of robot ****************************************************************************/ void Config::SetPosition(float x, float y, float z) { _pPosition[0] = x; _pPosition[1] = y, _pPosition[2] = z; } /**************************************************************************** Function : Config::SetPosition Description: Set position of robot ****************************************************************************/ void Config::SetPosition(Vec3f loc) { for(int i = 0; i < 3; i++) _pPosition[i] = loc[i]; } /**************************************************************************** Function : Config::SetRotation Description: Sets rotation quaternions of robot ****************************************************************************/ void Config::SetRotation(float* rot) { for(int i = 0; i < 4; i++) _pRotation[i] = rot[i]; } /**************************************************************************** Function : Config::SetRotation Description: Sets rotation quaternion of thr robot ****************************************************************************/ void Config::SetRotation(float q1, float q2, float q3, float q4) { _pRotation[0] = q1; _pRotation[1] = q2; _pRotation[2] = q3; _pRotation[3] = q4; } /**************************************************************************** Function : Config::IncrementPosition Description: Increment position of robot ****************************************************************************/ void Config::IncrementPosition(float* loc) { for(int i = 0; i < 3; i++) _pPosition[i] += loc[i]; } /**************************************************************************** Function : Config::IncrementPosition Description: Increments position fo robot ****************************************************************************/ void Config::IncrementPosition(float x, float y, float z) { _pPosition[0] += x; _pPosition[1] += y, _pPosition[2] += z; } /**************************************************************************** Function : Config::SetPosition Description: Increments position of robot ****************************************************************************/ void Config::IncrementPosition(Vec3f loc) { for(int i = 0; i < 3; i++) _pPosition[i] += loc[i]; } /**************************************************************************** Function : Config::IncrementRotation Description: Increments rotation quaternions of robot ****************************************************************************/ void Config::IncrementRotation(float* rot) { for(int i = 0; i < 4; i++) _pRotation[i] += rot[i]; } /**************************************************************************** Function : Config::IncrementRotation Description: Increments rotation quaternion of thr robot ****************************************************************************/ void Config::IncrementRotation(float q1, float q2, float q3, float q4) { _pRotation[0] += q1; _pRotation[1] += q2; _pRotation[2] += q3; _pRotation[3] += q4; } /**************************************************************************** Function : operator== Description: Equality comparison for two configurations ****************************************************************************/ bool operator==(const Config& a, const Config& b) { for (int i=0; i < 3; i++) if(fabs(a.GetPosition()[i] - b.GetPosition()[i]) > FLT_MIN) return false; for(i=0; i < 4; i++) if(fabs(a.GetRotation()[i] - b.GetRotation()[i]) > FLT_MIN) return false; return true; } /**************************************************************************** Function : operator= Description: Assignment operator. Only works for configs from the same domain. ****************************************************************************/ Config& Config:: operator=(const Config& b){ //if(_pDomain != b._pDomain) exit(1); for (int i=0; i < 3; i++) _pPosition[i] = b._pPosition[i]; for(i=0; i < 4; i++) _pRotation[i] = b._pRotation[i]; return *this; } /**************************************************************************** Function : Config::Midpoint Description: Computes midpoint between two configurations ****************************************************************************/ void Config::Midpoint(const Config& q0, const Config& q1) { for (int i=0; i < 3; i++) _pPosition[i] = (q0._pPosition[i] + q0._pPosition[i])/ 2.0; for(i=0; i < 4; i++) _pRotation[i] = (q0._pRotation[i] + q0._pRotation[i])/ 2.0; Normalize(); } /**************************************************************************** Function : Config::Normalize Description: Normalizes the rotational quaternions for the configuration ****************************************************************************/ void Config::Normalize() { float d = sqrt( _pRotation[0]*_pRotation[0] + _pRotation[1]*_pRotation[1] + _pRotation[2]*_pRotation[2] + _pRotation[3]*_pRotation[3] ); _pRotation[0] /= d; _pRotation[1] /= d; _pRotation[2] /= d; _pRotation[3] /= d; } /**************************************************************************** Function : Config::MatchHemisphere Description: Changes the calling configurations rotation to its antipode if it is not in the same hemisphere as p ****************************************************************************/ void Config::MatchHemisphere(const Config& p) { float d0,d1; d0= ( _pRotation[0] - p._pRotation[0] ) * ( _pRotation[0] - p._pRotation[0] ) + ( _pRotation[1] - p._pRotation[1] ) * ( _pRotation[1] - p._pRotation[1] ) + ( _pRotation[2] - p._pRotation[2] ) * ( _pRotation[2] - p._pRotation[2] ) + ( _pRotation[3] - p._pRotation[3] ) * ( _pRotation[3] - p._pRotation[3] ); d1= ( _pRotation[0] + p._pRotation[0] ) * ( _pRotation[0] + p._pRotation[0] ) + ( _pRotation[1] + p._pRotation[1] ) * ( _pRotation[1] + p._pRotation[1] ) + ( _pRotation[2] + p._pRotation[2] ) * ( _pRotation[2] + p._pRotation[2] ) + ( _pRotation[3] + p._pRotation[3] ) * ( _pRotation[3] + p._pRotation[3] ); if (d0 > d1) { Antipode(); } } /**************************************************************************** Function : Config::Antipode Description: Antipode of rotation ****************************************************************************/ void Config::Antipode() { _pRotation[0] = -_pRotation[0]; _pRotation[1] = -_pRotation[1]; _pRotation[2] = -_pRotation[2]; _pRotation[3] = -_pRotation[3]; } /* frame * convert the representation in C-space to Cartesian space * q is represented as (x,y,z, q0, q1, q2, q3) where (x,y,z) specifies * position and (q0, (q1,q2,q3)) is a unit quoternion * fs is represented as (x,y,z) and a 3x3 rotation matrix */ /**************************************************************************** Function : Config::Frame Description: Returns a frame which represents the tranlation and rotation of the configuration. The frame contains a 3-vector for position and a 3x3 rotation matrix ****************************************************************************/ void Config::Frame( Frame3& fs) const { fs.p[0] = _pPosition[0]; fs.p[1] = _pPosition[1]; fs.p[2] = _pPosition[2]; fs.r[0][0] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[1]*_pRotation[1] )-1; fs.r[0][1] = 2*( _pRotation[1]*_pRotation[2] - _pRotation[0]*_pRotation[3] ); fs.r[0][2] = 2*( _pRotation[1]*_pRotation[3] + _pRotation[0]*_pRotation[2] ); fs.r[1][0] = 2*( _pRotation[1]*_pRotation[2] + _pRotation[0]*_pRotation[3] ); fs.r[1][1] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[2]*_pRotation[2] )-1; fs.r[1][2] = 2*( _pRotation[2]*_pRotation[3] - _pRotation[0]*_pRotation[1] ); fs.r[2][0] = 2*( _pRotation[1]*_pRotation[3] - _pRotation[0]*_pRotation[2] ); fs.r[2][1] = 2*( _pRotation[2]*_pRotation[3] + _pRotation[0]*_pRotation[1] ); fs.r[2][2] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[3]*_pRotation[3] )-1; } /* glframe * convert the representation in C-space to homogeneous floatinates * and multiplay the current gl view matrix by the result. */ /**************************************************************************** Function : Config::GLFrame Description: Applies the transformation which represents the frame of the configuration to the OpenGL tranformation stack. ****************************************************************************/ void Config::GLFrame() const { float m[16]; m[0] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[1]*_pRotation[1] )-1; m[1] = 2*( _pRotation[1]*_pRotation[2] + _pRotation[0]*_pRotation[3] ); m[2] = 2*( _pRotation[1]*_pRotation[3] - _pRotation[0]*_pRotation[2] ); m[3] = 0; m[4] = 2*( _pRotation[1]*_pRotation[2] - _pRotation[0]*_pRotation[3] ); m[5] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[2]*_pRotation[2] )-1; m[6] = 2*( _pRotation[2]*_pRotation[3] + _pRotation[0]*_pRotation[1] ); m[7] = 0; m[8] = 2*( _pRotation[1]*_pRotation[3] + _pRotation[0]*_pRotation[2] ); m[9] = 2*( _pRotation[2]*_pRotation[3] - _pRotation[0]*_pRotation[1] ); m[10] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[3]*_pRotation[3] )-1; m[11] = 0; m[12] = _pPosition[0]; m[13] = _pPosition[1]; m[14] = _pPosition[2]; m[15] = 1; glMultMatrixf(m); } Mat44f Config::GetTransformationMatrix() const { float m[16]; m[0] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[1]*_pRotation[1] )-1; m[1] = 2*( _pRotation[1]*_pRotation[2] + _pRotation[0]*_pRotation[3] ); m[2] = 2*( _pRotation[1]*_pRotation[3] - _pRotation[0]*_pRotation[2] ); m[3] = 0; m[4] = 2*( _pRotation[1]*_pRotation[2] - _pRotation[0]*_pRotation[3] ); m[5] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[2]*_pRotation[2] )-1; m[6] = 2*( _pRotation[2]*_pRotation[3] + _pRotation[0]*_pRotation[1] ); m[7] = 0; m[8] = 2*( _pRotation[1]*_pRotation[3] + _pRotation[0]*_pRotation[2] ); m[9] = 2*( _pRotation[2]*_pRotation[3] - _pRotation[0]*_pRotation[1] ); m[10] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[3]*_pRotation[3] )-1; m[11] = 0; m[12] = _pPosition[0]; m[13] = _pPosition[1]; m[14] = _pPosition[2]; m[15] = 1; return Mat44f(m); } Mat33f Config::GetRotationMatrix() const { float m[9]; m[0] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[1]*_pRotation[1] )-1; m[1] = 2*( _pRotation[1]*_pRotation[2] + _pRotation[0]*_pRotation[3] ); m[2] = 2*( _pRotation[1]*_pRotation[3] - _pRotation[0]*_pRotation[2] ); m[3] = 2*( _pRotation[1]*_pRotation[2] - _pRotation[0]*_pRotation[3] ); m[4] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[2]*_pRotation[2] )-1; m[5] = 2*( _pRotation[2]*_pRotation[3] + _pRotation[0]*_pRotation[1] ); m[6] = 2*( _pRotation[1]*_pRotation[3] + _pRotation[0]*_pRotation[2] ); m[7] = 2*( _pRotation[2]*_pRotation[3] - _pRotation[0]*_pRotation[1] ); m[8] = 2*( _pRotation[0]*_pRotation[0] + _pRotation[3]*_pRotation[3] )-1; return Mat33f(m); } /**************************************************************************** Function : Config::Transform Description: Transform a point by the configuration ****************************************************************************/ Vec3f Config::Transform(Vec3f point) const { return GetTransformationMatrix() * point; }