Panda3D
angularEulerIntegrator.cxx
1 // Filename: angularEulerIntegrator.cxx
2 // Created by: charles (09Aug00)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "angularEulerIntegrator.h"
16 #include "forceNode.h"
17 #include "physicalNode.h"
18 #include "config_physics.h"
19 
20 ////////////////////////////////////////////////////////////////////
21 // Function : AngularEulerIntegrator
22 // Access : Public
23 // Description : constructor
24 ////////////////////////////////////////////////////////////////////
27 }
28 
29 ////////////////////////////////////////////////////////////////////
30 // Function : AngularEulerIntegrator
31 // Access : Public
32 // Description : destructor
33 ////////////////////////////////////////////////////////////////////
36 }
37 
38 ////////////////////////////////////////////////////////////////////
39 // Function : Integrate
40 // Access : Public
41 // Description : Integrate a step of motion (based on dt) by
42 // applying every force in force_vec to every object
43 // in obj_vec.
44 ////////////////////////////////////////////////////////////////////
45 void AngularEulerIntegrator::
46 child_integrate(Physical *physical,
47  AngularForceVector& forces,
48  PN_stdfloat dt) {
49  // Loop through each object in the set. This processing occurs in
50  // O(pf) time, where p is the number of physical objects and f is
51  // the number of forces. Unfortunately, no precomputation of forces
52  // can occur, as each force is possibly contingent on such things as
53  // the position and velocity of each physicsobject in the set.
54  // Accordingly, we have to grunt our way through each one. wrt
55  // caching of the xform matrix should help.
56  PhysicsObject::Vector::const_iterator current_object_iter;
57  current_object_iter = physical->get_object_vector().begin();
58  for (; current_object_iter != physical->get_object_vector().end();
59  ++current_object_iter) {
60  PhysicsObject *current_object = *current_object_iter;
61 
62  // bail out if this object doesn't exist or doesn't want to be
63  // processed.
64  if (current_object == (PhysicsObject *) NULL) {
65  continue;
66  }
67 
68  if (current_object->get_active() == false) {
69  continue;
70  }
71 
72  LRotation accum_quat(0, 0, 0, 0);
73 
74  // set up the traversal stuff.
75  AngularForceVector::const_iterator f_cur;
76 
77  LRotation f;
78 
79  // global forces
80  f_cur = forces.begin();
81  // unsigned int index = 0;
82  for (; f_cur != forces.end(); ++f_cur) {
83  AngularForce *cur_force = *f_cur;
84 
85  // make sure the force is turned on.
86  if (cur_force->get_active() == false) {
87  continue;
88  }
89 
90  // tally it into the accumulation quaternion
91  f = cur_force->get_quat(current_object);
92  accum_quat += f;
93  }
94 
95  LOrientation orientation = current_object->get_orientation();
96  // local forces
97  f_cur = physical->get_angular_forces().begin();
98  for (; f_cur != physical->get_angular_forces().end(); ++f_cur) {
99  AngularForce *cur_force = *f_cur;
100 
101  // make sure the force is turned on.
102  if (cur_force->get_active() == false) {
103  continue;
104  }
105 
106  f = cur_force->get_quat(current_object);
107 
108  // tally it into the accumulation quaternion
109  // i.e. orientation * f * orientation.conjugate()
110  accum_quat += orientation.xform(f);
111  }
112 
113  // apply the accumulated torque vector to the object's inertial tensor.
114  // this matrix represents how much force the object 'wants' applied to it
115  // in any direction, among other things.
116  accum_quat = current_object->get_inertial_tensor() * accum_quat;
117 
118  // derive this into the angular velocity vector.
119  LRotation rot_quat = current_object->get_rotation();
120  #if 0
121  rot_quat += accum_quat * dt;
122 
123  if (rot_quat.normalize()) {
124  LOrientation old_orientation = current_object->get_orientation();
125  LOrientation new_orientation = old_orientation * rot_quat;
126  new_orientation.normalize();
127 
128  // and write the results back.
129  current_object->set_orientation(new_orientation);
130  current_object->set_rotation(rot_quat);
131  }
132  #else
133  //accum_quat*=viscosityDamper;
134  //LOrientation orientation = current_object->get_orientation();
135 
136  //accum_quat.normalize();
137  // x = x + v * t + 0.5 * a * t * t
138  orientation = orientation * ((rot_quat * dt) * (accum_quat * (0.5 * dt * dt)));
139  // v = v + a * t
140  rot_quat = rot_quat + (accum_quat * dt);
141 
142  //if (rot_quat.normalize()) {
143  if (orientation.normalize() && rot_quat.normalize()) {
144  // and write the results back.
145  current_object->set_orientation(orientation);
146  current_object->set_rotation(rot_quat);
147  }
148  #endif
149  }
150 }
151 
152 ////////////////////////////////////////////////////////////////////
153 // Function : output
154 // Access : Public
155 // Description : Write a string representation of this instance to
156 // <out>.
157 ////////////////////////////////////////////////////////////////////
159 output(ostream &out) const {
160  #ifndef NDEBUG //[
161  out<<"AngularEulerIntegrator (id "<<this<<")";
162  #endif //] NDEBUG
163 }
164 
165 ////////////////////////////////////////////////////////////////////
166 // Function : write
167 // Access : Public
168 // Description : Write a string representation of this instance to
169 // <out>.
170 ////////////////////////////////////////////////////////////////////
172 write(ostream &out, unsigned int indent) const {
173  #ifndef NDEBUG //[
174  out.width(indent); out<<""; out<<"AngularEulerIntegrator:\n";
175  AngularIntegrator::write(out, indent+2);
176  #endif //] NDEBUG
177 }
LOrientation get_orientation() const
get current orientation.
This is a unit quaternion representing a rotation.
Definition: lrotation.h:92
bool get_active() const
Process Flag Query.
virtual void output(ostream &out) const
Write a string representation of this instance to <out>.
A body on which physics will be applied.
Definition: physicsObject.h:29
This is a unit quaternion representing an orientation.
Definition: lorientation.h:92
virtual void write(ostream &out, unsigned int indent=0) const
Write a string representation of this instance to <out>.
LRotation get_rotation() const
get rotation per second.
LVecBase3f xform(const LVecBase3f &v) const
Transforms a 3-d vector by the indicated rotation.
Definition: lquaternion.h:280
Defines a set of physically modeled attributes.
Definition: physical.h:40
virtual LMatrix4 get_inertial_tensor() const
returns a transform matrix that represents the object&#39;s willingness to be forced. ...
virtual ~AngularEulerIntegrator()
destructor
LRotation get_quat(const PhysicsObject *po)
access query
pure virtual parent of all quat-based forces.
Definition: angularForce.h:24
void set_rotation(const LRotation &rotation)
set rotation as a quaternion delta per second.
virtual void write(ostream &out, unsigned int indent=0) const
Write a string representation of this instance to <out>.