Panda3D
|
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 }