Panda3D
 All Classes Functions Variables Enumerations
angularEulerIntegrator.cxx
00001 // Filename: angularEulerIntegrator.cxx
00002 // Created by:  charles (09Aug00)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "angularEulerIntegrator.h"
00016 #include "forceNode.h"
00017 #include "physicalNode.h"
00018 #include "config_physics.h"
00019 
00020 ////////////////////////////////////////////////////////////////////
00021 //     Function : AngularEulerIntegrator
00022 //       Access : Public
00023 //  Description : constructor
00024 ////////////////////////////////////////////////////////////////////
00025 AngularEulerIntegrator::
00026 AngularEulerIntegrator() {
00027 }
00028 
00029 ////////////////////////////////////////////////////////////////////
00030 //     Function : AngularEulerIntegrator
00031 //       Access : Public
00032 //  Description : destructor
00033 ////////////////////////////////////////////////////////////////////
00034 AngularEulerIntegrator::
00035 ~AngularEulerIntegrator() {
00036 }
00037 
00038 ////////////////////////////////////////////////////////////////////
00039 //     Function : Integrate
00040 //       Access : Public
00041 //  Description : Integrate a step of motion (based on dt) by
00042 //                applying every force in force_vec to every object
00043 //                in obj_vec.
00044 ////////////////////////////////////////////////////////////////////
00045 void AngularEulerIntegrator::
00046 child_integrate(Physical *physical,
00047                 AngularForceVector& forces,
00048                 PN_stdfloat dt) {
00049   // Loop through each object in the set.  This processing occurs in
00050   // O(pf) time, where p is the number of physical objects and f is
00051   // the number of forces.  Unfortunately, no precomputation of forces
00052   // can occur, as each force is possibly contingent on such things as
00053   // the position and velocity of each physicsobject in the set.
00054   // Accordingly, we have to grunt our way through each one.  wrt
00055   // caching of the xform matrix should help.
00056   PhysicsObject::Vector::const_iterator current_object_iter;
00057   current_object_iter = physical->get_object_vector().begin();
00058   for (; current_object_iter != physical->get_object_vector().end();
00059        ++current_object_iter) {
00060     PhysicsObject *current_object = *current_object_iter;
00061 
00062     // bail out if this object doesn't exist or doesn't want to be
00063     // processed.
00064     if (current_object == (PhysicsObject *) NULL) {
00065       continue;
00066     }
00067 
00068     if (current_object->get_active() == false) {
00069       continue;
00070     }
00071 
00072     LRotation accum_quat(0, 0, 0, 0);
00073 
00074     // set up the traversal stuff.
00075     AngularForceVector::const_iterator f_cur;
00076 
00077     LRotation f;
00078 
00079     // global forces
00080     f_cur = forces.begin();
00081     //    unsigned int index = 0;
00082     for (; f_cur != forces.end(); ++f_cur) {
00083       AngularForce *cur_force = *f_cur;
00084 
00085       // make sure the force is turned on.
00086       if (cur_force->get_active() == false) {
00087         continue;
00088       }
00089 
00090       // tally it into the accumulation quaternion
00091       f = cur_force->get_quat(current_object);
00092       accum_quat += f;
00093     }
00094 
00095     LOrientation orientation = current_object->get_orientation();
00096     // local forces
00097     f_cur = physical->get_angular_forces().begin();
00098     for (; f_cur != physical->get_angular_forces().end(); ++f_cur) {
00099       AngularForce *cur_force = *f_cur;
00100 
00101       // make sure the force is turned on.
00102       if (cur_force->get_active() == false) {
00103         continue;
00104       }
00105 
00106       f = cur_force->get_quat(current_object);
00107 
00108       // tally it into the accumulation quaternion
00109       // i.e. orientation * f * orientation.conjugate()
00110       accum_quat += orientation.xform(f);
00111     }
00112 
00113     // apply the accumulated torque vector to the object's inertial tensor.
00114     // this matrix represents how much force the object 'wants' applied to it
00115     // in any direction, among other things.
00116     accum_quat = current_object->get_inertial_tensor() * accum_quat;
00117 
00118     // derive this into the angular velocity vector.
00119     LRotation rot_quat = current_object->get_rotation();
00120     #if 0
00121     rot_quat += accum_quat * dt;
00122 
00123     if (rot_quat.normalize()) {
00124       LOrientation old_orientation = current_object->get_orientation();
00125       LOrientation new_orientation = old_orientation * rot_quat;
00126       new_orientation.normalize();
00127 
00128       // and write the results back.
00129       current_object->set_orientation(new_orientation);
00130       current_object->set_rotation(rot_quat);
00131     }
00132     #else
00133     //accum_quat*=viscosityDamper;
00134     //LOrientation orientation = current_object->get_orientation();
00135     
00136     //accum_quat.normalize();
00137     // x = x + v * t + 0.5 * a * t * t
00138     orientation = orientation * ((rot_quat * dt) * (accum_quat * (0.5 * dt * dt)));
00139     // v = v + a * t
00140     rot_quat = rot_quat + (accum_quat * dt);
00141 
00142     //if (rot_quat.normalize()) {
00143     if (orientation.normalize() && rot_quat.normalize()) {
00144       // and write the results back.
00145       current_object->set_orientation(orientation);
00146       current_object->set_rotation(rot_quat);
00147     }
00148     #endif
00149   }
00150 }
00151 
00152 ////////////////////////////////////////////////////////////////////
00153 //     Function : output
00154 //       Access : Public
00155 //  Description : Write a string representation of this instance to
00156 //                <out>.
00157 ////////////////////////////////////////////////////////////////////
00158 void AngularEulerIntegrator::
00159 output(ostream &out) const {
00160   #ifndef NDEBUG //[
00161   out<<"AngularEulerIntegrator (id "<<this<<")";
00162   #endif //] NDEBUG
00163 }
00164 
00165 ////////////////////////////////////////////////////////////////////
00166 //     Function : write
00167 //       Access : Public
00168 //  Description : Write a string representation of this instance to
00169 //                <out>.
00170 ////////////////////////////////////////////////////////////////////
00171 void AngularEulerIntegrator::
00172 write(ostream &out, unsigned int indent) const {
00173   #ifndef NDEBUG //[
00174   out.width(indent); out<<""; out<<"AngularEulerIntegrator:\n";
00175   AngularIntegrator::write(out, indent+2);
00176   #endif //] NDEBUG
00177 }
 All Classes Functions Variables Enumerations