Panda3D
 All Classes Functions Variables Enumerations
linearEulerIntegrator.cxx
1 // Filename: linearEulerIntegrator.cxx
2 // Created by: charles (13Jun00)
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 "linearEulerIntegrator.h"
16 #include "forceNode.h"
17 #include "physicalNode.h"
18 #include "config_physics.h"
19 
20 ////////////////////////////////////////////////////////////////////
21 // Function : LinearEulerIntegrator
22 // Access : Public
23 // Description : constructor
24 ////////////////////////////////////////////////////////////////////
27 }
28 
29 ////////////////////////////////////////////////////////////////////
30 // Function : LinearEulerIntegrator
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 // physical,
46 // The objects being acted upon and the
47 // set of local forces that are applied
48 // after the global forces.
49 // forces,
50 // Global forces to be applied first.
51 // dt,
52 // The delta time of this integration step.
53 ////////////////////////////////////////////////////////////////////
54 void LinearEulerIntegrator::
55 child_integrate(Physical *physical,
56  LinearForceVector& forces,
57  PN_stdfloat dt) {
58  // perform the precomputation. Note that the vector returned by
59  // get_precomputed_matrices() has the matrices loaded in order of force
60  // type: first global, then local. If you're using this as a guide to write
61  // another integrator, be sure to process your forces global, then local.
62  // otherwise your transforms will be VERY bad.
63  precompute_linear_matrices(physical, forces);
64  const MatrixVector &matrices = get_precomputed_linear_matrices();
65 #ifndef NDEBUG
66  MatrixVector::const_iterator mi;
67  for (mi = matrices.begin(); mi != matrices.end(); ++mi) {
68  nassertv(!(*mi).is_nan());
69  }
70 #endif // NDEBUG
71 
72  // Get the greater of the local or global viscosity:
73  PN_stdfloat viscosityDamper=1.0f-physical->get_viscosity();
74 
75  // Loop through each object in the set. This processing occurs in O(pf) time,
76  // where p is the number of physical objects and f is the number of
77  // forces. Unfortunately, no precomputation of forces can occur, as
78  // each force is possibly contingent on such things as the position and
79  // velocity of each physicsobject in the set. Accordingly, we have
80  // to grunt our way through each one. wrt caching of the xform matrix
81  // should help.
82  PhysicsObject::Vector::const_iterator current_object_iter;
83  current_object_iter = physical->get_object_vector().begin();
84  for (; current_object_iter != physical->get_object_vector().end();
85  ++current_object_iter) {
86  PhysicsObject *current_object = *current_object_iter;
87 
88  // bail out if this object doesn't exist or doesn't want to be
89  // processed.
90  if (current_object == (PhysicsObject *) NULL) {
91  continue;
92  }
93 
94  if (current_object->get_active() == false) {
95  continue;
96  }
97 
98  LVector3 md_accum_vec; // mass dependent accumulation vector.
99  LVector3 non_md_accum_vec;
100  LVector3 accel_vec;
101  LVector3 vel_vec;
102 
103  // reset the accumulation vectors for this object
104  md_accum_vec.set(0.0f, 0.0f, 0.0f);
105  non_md_accum_vec.set(0.0f, 0.0f, 0.0f);
106 
107  // run through each acting force and sum it
108  LVector3 f;
109  // LMatrix4 force_to_object_xform;
110 
111  LinearForceVector::const_iterator f_cur;
112 
113  // global forces
114  f_cur = forces.begin();
115  int index = 0;
116  for (; f_cur != forces.end(); ++f_cur) {
117  LinearForce *cur_force = *f_cur;
118 
119  // make sure the force is turned on.
120  if (cur_force->get_active() == false) {
121  continue;
122  }
123 
124  // now we go from force space to our object's space.
125  f = cur_force->get_vector(current_object) * matrices[index++];
126 
127  physics_spam("child_integrate "<<f);
128  // tally it into the accum vectors.
129  if (cur_force->get_mass_dependent() == true) {
130  md_accum_vec += f;
131  } else {
132  non_md_accum_vec += f;
133  }
134  }
135 
136  // local forces
137  f_cur = physical->get_linear_forces().begin();
138  for (; f_cur != physical->get_linear_forces().end(); ++f_cur) {
139  LinearForce *cur_force = *f_cur;
140 
141  // make sure the force is turned on.
142  if (cur_force->get_active() == false) {
143  continue;
144  }
145 
146  // go from force space to object space
147  f = cur_force->get_vector(current_object) * matrices[index++];
148 
149  physics_spam("child_integrate "<<f);
150  // tally it into the accum vectors
151  if (cur_force->get_mass_dependent() == true) {
152  md_accum_vec += f;
153  } else {
154  non_md_accum_vec += f;
155  }
156  }
157 
158  // get this object's physical info
159  LPoint3 pos = current_object->get_position();
160  vel_vec = current_object->get_velocity();
161  PN_stdfloat mass = current_object->get_mass();
162 
163  // we want 'a' in F = ma
164  // get it by computing F / m
165  nassertv(mass != 0.0f);
166  accel_vec = md_accum_vec / mass;
167  accel_vec += non_md_accum_vec;
168 
169  #if 0 //[
170  // step the position and velocity
171  vel_vec += accel_vec * dt;
172 
173  // cap terminal velocity
174  PN_stdfloat len = vel_vec.length();
175 
176  if (len > current_object->get_terminal_velocity()) {
177  //cout << "Capping terminal velocity at: " << current_object->get_terminal_velocity() << endl;
178  vel_vec *= current_object->get_terminal_velocity() / len;
179  }
180 
181  pos += vel_vec * dt;
182  #else //][
183  assert(current_object->get_position()==current_object->get_last_position());
184 
185  accel_vec*=viscosityDamper;
186 
187  // x = x + v * t + 0.5 * a * t * t
188  pos += vel_vec * dt + 0.5 * accel_vec * dt * dt;
189  // v = v + a * t
190  vel_vec += accel_vec * dt;
191  #endif //]
192 
193  // and store them back.
194  if (!pos.is_nan()) {
195  current_object->set_position(pos);
196  }
197  if (!vel_vec.is_nan()) {
198  current_object->set_velocity(vel_vec);
199  }
200  }
201 }
202 
203 ////////////////////////////////////////////////////////////////////
204 // Function : output
205 // Access : Public
206 // Description : Write a string representation of this instance to
207 // <out>.
208 ////////////////////////////////////////////////////////////////////
210 output(ostream &out) const {
211  #ifndef NDEBUG //[
212  out<<"LinearEulerIntegrator";
213  #endif //] NDEBUG
214 }
215 
216 ////////////////////////////////////////////////////////////////////
217 // Function : write
218 // Access : Public
219 // Description : Write a string representation of this instance to
220 // <out>.
221 ////////////////////////////////////////////////////////////////////
223 write(ostream &out, unsigned int indent) const {
224  #ifndef NDEBUG //[
225  out.width(indent);
226  out<<""<<"LinearEulerIntegrator:\n";
227  LinearIntegrator::write(out, indent+2);
228  #endif //] NDEBUG
229 }
230 
231 
232 
233 
234 
235 
236 
237 
LPoint3 get_position() const
Position Query.
LVector3 get_velocity() const
Velocity Query per second.
void set_position(const LPoint3 &pos)
Vector position assignment.
Definition: physicsObject.I:33
LPoint3 get_last_position() const
Get the position of the physics object at the start of the most recent do_physics.
bool get_active() const
Process Flag Query.
A body on which physics will be applied.
Definition: physicsObject.h:29
void set_velocity(const LVector3 &vel)
Vector velocity assignment.
Definition: physicsObject.I:93
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
bool is_nan() const
Returns true if any component of the vector is not-a-number, false otherwise.
Definition: lvecBase3.h:463
A force that acts on a PhysicsObject by way of an Integrator.
Definition: linearForce.h:25
float length() const
Returns the length of the vector, by the Pythagorean theorem.
Definition: lvecBase3.h:765
virtual void write(ostream &out, unsigned int indent=0) const
Write a string representation of this instance to &lt;out&gt;.
virtual ~LinearEulerIntegrator()
destructor
PN_stdfloat get_mass() const
Get the mass in slugs (or kilograms).
PN_stdfloat get_terminal_velocity() const
tv query
PN_stdfloat get_viscosity() const
Get the local viscosity.
Definition: physical.I:235
Defines a set of physically modeled attributes.
Definition: physical.h:40
virtual void write(ostream &out, unsigned int indent=0) const
Write a string representation of this instance to &lt;out&gt;.
virtual void output(ostream &out) const
Write a string representation of this instance to &lt;out&gt;.