open_toontown_panda3d/panda/src/physics/linearEulerIntegrator.cxx

// Filename: LinearEulerIntegrator.cxx
// Created by:  charles (13Jun00)
//
////////////////////////////////////////////////////////////////////

#include "linearEulerIntegrator.h"
#include "forceNode.h"
#include "physicalNode.h"
#include "config_physics.h"

#include <get_rel_pos.h>

////////////////////////////////////////////////////////////////////
//     Function : LinearEulerIntegrator
//       Access : Public
//  Description : constructor
////////////////////////////////////////////////////////////////////
LinearEulerIntegrator::
LinearEulerIntegrator(void) {
}

////////////////////////////////////////////////////////////////////
//     Function : LinearEulerIntegrator
//       Access : Public
//  Description : destructor
////////////////////////////////////////////////////////////////////
LinearEulerIntegrator::
~LinearEulerIntegrator(void) {
}

////////////////////////////////////////////////////////////////////
//     Function : Integrate
//       Access : Public
//  Description : Integrate a step of motion (based on dt) by
//                applying every force in force_vec to every object
//                in obj_vec.
////////////////////////////////////////////////////////////////////
void LinearEulerIntegrator::
child_integrate(Physical *physical,
		vector< PT(LinearForce) >& forces,
		float dt) {
  vector< PT(PhysicsObject) >::const_iterator current_object_iter;

  // perform the precomputation.  Note that the vector returned by
  // get_precomputed_matrices() has the matrices loaded in order of force
  // type: first global, then local.  If you're using this as a guide to write
  // another integrator, be sure to process your forces global, then local.
  // otherwise your transforms will be VERY bad.
  precompute_linear_matrices(physical, forces);
  const vector< LMatrix4f > &matrices = get_precomputed_linear_matrices();

  // Loop through each object in the set.  This processing occurs in O(pf) time,
  // where p is the number of physical objects and f is the number of
  // forces.  Unfortunately, no precomputation of forces can occur, as
  // each force is possibly contingent on such things as the position and
  // velocity of each physicsobject in the set.  Accordingly, we have
  // to grunt our way through each one.  wrt caching of the xform matrix
  // should help.

  current_object_iter = physical->get_object_vector().begin();
  for (; current_object_iter != physical->get_object_vector().end();
       current_object_iter++) {
    LVector3f md_accum_vec, non_md_accum_vec, accel_vec, vel_vec;
    LPoint3f pos;
    float mass;

    PhysicsObject *current_object = *current_object_iter;

    // bail out if this object doesn't exist or doesn't want to be
    // processed.
    if (current_object == (PhysicsObject *) NULL)
      continue;

    if (current_object->get_active() == false)
      continue;

    // reset the accumulation vectors for this object
    md_accum_vec.set(0.0f, 0.0f, 0.0f);
    non_md_accum_vec.set(0.0f, 0.0f, 0.0f);

    // run through each acting force and sum it
    LVector3f f;
    //    LMatrix4f force_to_object_xform;

    ForceNode *force_node;
    vector< PT(LinearForce) >::const_iterator f_cur;

    // global forces
    f_cur = forces.begin();
    int index = 0;
    for (; f_cur != forces.end(); f_cur++) {
      LinearForce *cur_force = *f_cur;

      // make sure the force is turned on.
      if (cur_force->get_active() == false)
	continue;

      force_node = cur_force->get_force_node();

      // now we go from force space to our object's space.
      f = matrices[index++] * cur_force->get_vector(current_object);

      // tally it into the accum vectors.
      if (cur_force->get_mass_dependent() == true)
	md_accum_vec += f;
      else
	non_md_accum_vec += f;
    }

    // local forces
    f_cur = physical->get_linear_forces().begin();
    for (; f_cur != physical->get_linear_forces().end(); f_cur++) {
      LinearForce *cur_force = *f_cur;

      // make sure the force is turned on.
      if (cur_force->get_active() == false)
	continue;

      force_node = cur_force->get_force_node();

      // go from force space to object space
      f = matrices[index++] * cur_force->get_vector(current_object);

      // tally it into the accum vectors
      if (cur_force->get_mass_dependent() == true)
	md_accum_vec += f;
      else
	non_md_accum_vec += f;
    }

    // get this object's physical info
    pos = current_object->get_position();
    vel_vec = current_object->get_velocity();
    mass = current_object->get_mass();

    // we want 'a' in F = ma
    // get it by computing F / m
    nassertv(mass != 0.0f);

    accel_vec = md_accum_vec / mass;
    accel_vec += non_md_accum_vec;

    // step the position and velocity
    vel_vec += accel_vec * dt;

    // cap terminal velocity
    float len = vel_vec.length();

    if (len > current_object->get_terminal_velocity()) {
      cout << "Capping terminal velocity at: " << current_object->get_terminal_velocity() << endl;
      vel_vec *= current_object->get_terminal_velocity() / len;
    }

    pos += vel_vec * dt;

    // and store them back.
    current_object->set_position(pos);
    current_object->set_velocity(vel_vec);
  }
}