Panda3D

linearEulerIntegrator.cxx

00001 // Filename: linearEulerIntegrator.cxx
00002 // Created by:  charles (13Jun00)
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 "linearEulerIntegrator.h"
00016 #include "forceNode.h"
00017 #include "physicalNode.h"
00018 #include "config_physics.h"
00019 
00020 ////////////////////////////////////////////////////////////////////
00021 //     Function : LinearEulerIntegrator
00022 //       Access : Public
00023 //  Description : constructor
00024 ////////////////////////////////////////////////////////////////////
00025 LinearEulerIntegrator::
00026 LinearEulerIntegrator() {
00027 }
00028 
00029 ////////////////////////////////////////////////////////////////////
00030 //     Function : LinearEulerIntegrator
00031 //       Access : Public
00032 //  Description : destructor
00033 ////////////////////////////////////////////////////////////////////
00034 LinearEulerIntegrator::
00035 ~LinearEulerIntegrator() {
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 //                physical,
00046 //                    The objects being acted upon and the
00047 //                    set of local forces that are applied 
00048 //                    after the global forces.
00049 //                forces,
00050 //                    Global forces to be applied first.
00051 //                dt,
00052 //                    The delta time of this integration step.
00053 ////////////////////////////////////////////////////////////////////
00054 void LinearEulerIntegrator::
00055 child_integrate(Physical *physical,
00056                 LinearForceVector& forces,
00057                 PN_stdfloat dt) {
00058   // perform the precomputation.  Note that the vector returned by
00059   // get_precomputed_matrices() has the matrices loaded in order of force
00060   // type: first global, then local.  If you're using this as a guide to write
00061   // another integrator, be sure to process your forces global, then local.
00062   // otherwise your transforms will be VERY bad.
00063   precompute_linear_matrices(physical, forces);
00064   const MatrixVector &matrices = get_precomputed_linear_matrices();
00065 #ifndef NDEBUG
00066   MatrixVector::const_iterator mi;
00067   for (mi = matrices.begin(); mi != matrices.end(); ++mi) {
00068     nassertv(!(*mi).is_nan());
00069   }
00070 #endif  // NDEBUG
00071 
00072   // Get the greater of the local or global viscosity:
00073   PN_stdfloat viscosityDamper=1.0f-physical->get_viscosity();
00074 
00075   // Loop through each object in the set.  This processing occurs in O(pf) time,
00076   // where p is the number of physical objects and f is the number of
00077   // forces.  Unfortunately, no precomputation of forces can occur, as
00078   // each force is possibly contingent on such things as the position and
00079   // velocity of each physicsobject in the set.  Accordingly, we have
00080   // to grunt our way through each one.  wrt caching of the xform matrix
00081   // should help.
00082   PhysicsObject::Vector::const_iterator current_object_iter;
00083   current_object_iter = physical->get_object_vector().begin();
00084   for (; current_object_iter != physical->get_object_vector().end();
00085        ++current_object_iter) {
00086     PhysicsObject *current_object = *current_object_iter;
00087 
00088     // bail out if this object doesn't exist or doesn't want to be
00089     // processed.
00090     if (current_object == (PhysicsObject *) NULL) {
00091       continue;
00092     }
00093     
00094     if (current_object->get_active() == false) {
00095       continue;
00096     }
00097     
00098     LVector3 md_accum_vec; // mass dependent accumulation vector.
00099     LVector3 non_md_accum_vec;
00100     LVector3 accel_vec;
00101     LVector3 vel_vec;
00102     
00103     // reset the accumulation vectors for this object
00104     md_accum_vec.set(0.0f, 0.0f, 0.0f);
00105     non_md_accum_vec.set(0.0f, 0.0f, 0.0f);
00106 
00107     // run through each acting force and sum it
00108     LVector3 f;
00109     //    LMatrix4 force_to_object_xform;
00110 
00111     LinearForceVector::const_iterator f_cur;
00112 
00113     // global forces
00114     f_cur = forces.begin();
00115     int index = 0;
00116     for (; f_cur != forces.end(); ++f_cur) {
00117       LinearForce *cur_force = *f_cur;
00118 
00119       // make sure the force is turned on.
00120       if (cur_force->get_active() == false) {
00121         continue;
00122       }
00123 
00124       // now we go from force space to our object's space.
00125       f = cur_force->get_vector(current_object) * matrices[index++];
00126 
00127       physics_spam("child_integrate "<<f);
00128       // tally it into the accum vectors.
00129       if (cur_force->get_mass_dependent() == true) {
00130         md_accum_vec += f;
00131       } else {
00132         non_md_accum_vec += f;
00133       }
00134     }
00135 
00136     // local forces
00137     f_cur = physical->get_linear_forces().begin();
00138     for (; f_cur != physical->get_linear_forces().end(); ++f_cur) {
00139       LinearForce *cur_force = *f_cur;
00140 
00141       // make sure the force is turned on.
00142       if (cur_force->get_active() == false) {
00143         continue;
00144       }
00145 
00146       // go from force space to object space
00147       f = cur_force->get_vector(current_object) * matrices[index++];
00148 
00149       physics_spam("child_integrate "<<f);
00150       // tally it into the accum vectors
00151       if (cur_force->get_mass_dependent() == true) {
00152         md_accum_vec += f;
00153       } else {
00154         non_md_accum_vec += f;
00155       }
00156     }
00157 
00158     // get this object's physical info
00159     LPoint3 pos = current_object->get_position();
00160     vel_vec = current_object->get_velocity();
00161     PN_stdfloat mass = current_object->get_mass();
00162 
00163     // we want 'a' in F = ma
00164     // get it by computing F / m
00165     nassertv(mass != 0.0f);
00166     accel_vec = md_accum_vec / mass;
00167     accel_vec += non_md_accum_vec;
00168 
00169     #if 0 //[
00170     // step the position and velocity
00171     vel_vec += accel_vec * dt;
00172 
00173     // cap terminal velocity
00174     PN_stdfloat len = vel_vec.length();
00175 
00176     if (len > current_object->get_terminal_velocity()) {
00177       //cout << "Capping terminal velocity at: " << current_object->get_terminal_velocity() << endl;
00178       vel_vec *= current_object->get_terminal_velocity() / len;
00179     }
00180 
00181     pos += vel_vec * dt;
00182     #else //][
00183     assert(current_object->get_position()==current_object->get_last_position());
00184     
00185     accel_vec*=viscosityDamper;
00186     
00187     // x = x + v * t + 0.5 * a * t * t
00188     pos += vel_vec * dt + 0.5 * accel_vec * dt * dt;
00189     // v = v + a * t
00190     vel_vec += accel_vec * dt;
00191     #endif //]
00192     
00193     // and store them back.
00194     if (!pos.is_nan()) {
00195       current_object->set_position(pos);
00196     }
00197     if (!vel_vec.is_nan()) {
00198       current_object->set_velocity(vel_vec);
00199     }
00200   }
00201 }
00202 
00203 ////////////////////////////////////////////////////////////////////
00204 //     Function : output
00205 //       Access : Public
00206 //  Description : Write a string representation of this instance to
00207 //                <out>.
00208 ////////////////////////////////////////////////////////////////////
00209 void LinearEulerIntegrator::
00210 output(ostream &out) const {
00211   #ifndef NDEBUG //[
00212   out<<"LinearEulerIntegrator";
00213   #endif //] NDEBUG
00214 }
00215 
00216 ////////////////////////////////////////////////////////////////////
00217 //     Function : write
00218 //       Access : Public
00219 //  Description : Write a string representation of this instance to
00220 //                <out>.
00221 ////////////////////////////////////////////////////////////////////
00222 void LinearEulerIntegrator::
00223 write(ostream &out, unsigned int indent) const {
00224   #ifndef NDEBUG //[
00225   out.width(indent);
00226   out<<""<<"LinearEulerIntegrator:\n";
00227   LinearIntegrator::write(out, indent+2);
00228   #endif //] NDEBUG
00229 }
00230 
00231 
00232 
00233 
00234 
00235 
00236 
00237 
 All Classes Functions Variables Enumerations