/*************************************************************************** File: PBMProjectile.h Created: 02/06/2002 Author: Maxim Garber Computer Science Department University of North Carolina - Chapel Hill garber@cs.unc.edu Description: Projectile object for physically based modeling simulation. ----------------------------------------------------------------------------. Copyright 2001 Maxim Garber UNC Motion Planning Research Group *****************************************************************************/ #include "PBMProjectile.h" #include "vec3fv.hpp" #include #include #include "GL\gl.h" #include "GL\glu.h" #include "GL\glut.h" /**************************************************************************** Function : PBMProjectile Description: Constructor, reads in a file of geometry, computes collision geometry, and initializes state ****************************************************************************/ PBMProjectile:: PBMProjectile(char* geometryFile, PBMProjectileLaunchParams params) // inialize ODEsystem with 7 dimensions : ODESystem(7) { _geometry = new X_TriModel(geometryFile); _InitCollisionGeometry(); for(unsigned int i=0; i<3; i++)_color[i] = 0.3f; _Launch(params); } /**************************************************************************** Function : PBMProjectile Description: Constructor, for instanced redering geometry. Takes in rendering geometry, computes collision geometry and initializes the state. ****************************************************************************/ PBMProjectile:: PBMProjectile(X_TriModel* geometry, PBMProjectileLaunchParams params) // inialize ODEsystem with 7 dimensions : ODESystem(7) { _geometry = geometry; _InitCollisionGeometry(); for(unsigned int i=0; i<3; i++)_color[i] = 0.3f; _Launch(params); } /**************************************************************************** Function : PBMProjectile Description: Constructor, for instanced redering and collision geometry. Takes in rendering geometry, and collision geometry, and initializes the state. ****************************************************************************/ PBMProjectile:: PBMProjectile(X_TriModel* geometry, PQP_Model* collisionGeometry, PBMProjectileLaunchParams params) // inialize ODEsystem with 7 dimensions : ODESystem(7) { _geometry = geometry; _collisionGeometry = collisionGeometry; for(unsigned int i=0; i<3; i++)_color[i] = 0.3f; _Launch(params); } /**************************************************************************** Function : Update Description: Updates projectile position and velocity over one time step. ****************************************************************************/ void PBMProjectile::Update(float timeStep) { // pack the ODE configuration ODEConfig config; unsigned int i; for(i=0; i<3;i++) config.push_back(_config.GetPosition()[i]); for(i=0; i<4;i++) config.push_back(_config.GetRotation()[i]); // call the integrator ODEConfig newConfig; _integrator->SetSystem(this); _integrator->Integrate(config, 0.0, timeStep, newConfig); // unpack the ODE configuration float temp[4]; for(i=0; i<3; i++) temp[i] = newConfig[i]; _config.SetPosition(temp); // get rotation for(i=0; i<4; i++) temp[i] = newConfig[i+3]; _config.SetRotation(temp); _config.Normalize(); } /**************************************************************************** Function : Draw Description: Draws the projectile ****************************************************************************/ void PBMProjectile::Draw() { //draw sphere glPushMatrix(); glColor3fv(_color); _config.GLFrame(); if(_geometry && 0) _geometry->Render(); else glutSolidSphere(1.0, 20, 20); glPopMatrix(); } /**************************************************************************** Function : GetPosition Description: Returns the projectiles origin position. ****************************************************************************/ void PBMProjectile::GetPosition(float position[3]) { for(unsigned int i=0; i<3; i++) position[i] = _config.GetPosition()[i]; } /**************************************************************************** Function : SetPosition Description: Sets the projectiles origin position ****************************************************************************/ void PBMProjectile::SetPosition(float position[3]) { _config.SetPosition(position); } /**************************************************************************** Function : GetVelocity Description: Returns the projectiles linear velocity ****************************************************************************/ void PBMProjectile::GetVelocity(float velocity[3]) { for(unsigned int i=0; i<3; i++) velocity[i] = _linVelocity[i]; } /**************************************************************************** Function : SetVelocity Description: Sets the projectiles linear velocity ****************************************************************************/ void PBMProjectile::SetVelocity(float velocity[3]) { for(unsigned int i=0; i<3; i++) _linVelocity[i] = velocity[i]; } /**************************************************************************** Function : GetCollisionGeometry Description: Returns the collision geometry and transformation frame. ****************************************************************************/ void PBMProjectile::GetCollisionGeometry(PQP_Model* &geometry, Frame3& frame) { geometry = _collisionGeometry; // set frame _config.Frame(frame); } /**************************************************************************** Function : QuatMul Description: quaternion multiplication r = q p Don't do QuatMul(q, p, q) !!! ****************************************************************************/ inline void QuatMul(float* r, const float* const q, const float* const p) { r[0] = q[0]*p[0] - q[1]*p[1] - q[2]*p[2] - q[3]*p[3]; r[1] = q[0]*p[1] + q[1]*p[0] + q[2]*p[3] - q[3]*p[2]; r[2] = q[0]*p[2] - q[1]*p[3] + q[2]*p[0] + q[3]*p[1]; r[3] = q[0]*p[3] + q[1]*p[2] - q[2]*p[1] + q[3]*p[0]; } /**************************************************************************** Function : ODEDerivative Description: A Callback for the ODE integrator which provides the integrator with the derivative for the state (position and velocity) of the projectile. ****************************************************************************/ void PBMProjectile::ODEDerivative(const ODEConfig& config, float time, ODEConfig& derivative) { derivative.clear(); unsigned int i; ///////////////////////////////////////// // derivative of position is the velocity for(i=0;i<3;i++) derivative.push_back(_linVelocity[i]); //////////////////////////////////////////////// // angular velocity is the derivative of rotation // convert the current angular velocity to a quaternion float omega[4] = {0, _angVelocity[0], _angVelocity[1], _angVelocity[2]}; // compute the derivative of the rotation in quaternion space by the formula // rot_prime = 0.5*omega*rotation float rot_prime[4], rot[4]; for(i=0; i<4; i++) rot[i] = config[i+3]; QuatMul(rot_prime, omega, rot); // set rot_prime as the direvative of rotation for(i=0; i<4; i++) derivative.push_back(0.5*rot_prime[i]); } /**************************************************************************** Function : _Launch Description: Initializes state and velocities from a struct of parameters ****************************************************************************/ void PBMProjectile::_Launch(PBMProjectileLaunchParams params) { // copy launch position _config.SetPosition(params.position); // velocity unit vector Copy3fv(_linVelocity, params.linVelocityDirection); // launch velocity magnitude ScalarMult3fv(_linVelocity, _linVelocity, params.linVelocityMagnitude); _config.SetRotation(params.rotation); _config.Normalize(); for(unsigned int i=0; i<3; i++) _angVelocity[i] = params.angVelocity[i]; // initialize ODE solver _integrator = params.integrator; } /**************************************************************************** Function : _InitCollisionGeometry Description: Computes collision geometry from a renderable model ****************************************************************************/ void PBMProjectile::_InitCollisionGeometry() { _collisionGeometry = new PQP_Model(); _collisionGeometry->BeginModel(); const float* vertexArray = _geometry->GetVertexArray(); const unsigned int* indexArray = _geometry->GetIndexArray(); for (int k=0; k<3*_geometry->GetNumTriangles(); k+=3) { float A[3] = {vertexArray[3*indexArray[k ] ], vertexArray[3*indexArray[k ]+1], vertexArray[3*indexArray[k ]+2]}; float B[3] = {vertexArray[3*indexArray[k+1] ], vertexArray[3*indexArray[k+1]+1], vertexArray[3*indexArray[k+1]+2]}; float C[3] = {vertexArray[3*indexArray[k+2] ], vertexArray[3*indexArray[k+2]+1], vertexArray[3*indexArray[k+2]+2]}; _collisionGeometry->AddTri(A, B, C, k/3); } _collisionGeometry->EndModel(); }