Panda3D

linearEulerIntegrator.cxx
Go to the documentation of this file.
1 /**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file linearEulerIntegrator.cxx
10 * @author charles
11 * @date 2000-06-13
12 */
13
14 #include "linearEulerIntegrator.h"
15 #include "forceNode.h"
16 #include "physicalNode.h"
17 #include "config_physics.h"
18
19 /**
20 * constructor
21 */
22 LinearEulerIntegrator::
23 LinearEulerIntegrator() {
24 }
25
26 /**
27 * destructor
28 */
29 LinearEulerIntegrator::
30 ~LinearEulerIntegrator() {
31 }
32
33 /**
34 * Integrate a step of motion (based on dt) by applying every force in
35 * force_vec to every object in obj_vec.
36 *
37 * physical, The objects being acted upon and the set of local forces that are
38 * applied after the global forces. forces, Global forces to be applied
39 * first. dt, The delta time of this integration step.
40 */
41 void LinearEulerIntegrator::
42 child_integrate(Physical *physical,
43 LinearForceVector& forces,
44 PN_stdfloat dt) {
45 // perform the precomputation. Note that the vector returned by
46 // get_precomputed_matrices() has the matrices loaded in order of force
47 // type: first global, then local. If you're using this as a guide to write
48 // another integrator, be sure to process your forces global, then local.
49 // otherwise your transforms will be VERY bad.
50 precompute_linear_matrices(physical, forces);
51 const MatrixVector &matrices = get_precomputed_linear_matrices();
52 #ifndef NDEBUG
53 MatrixVector::const_iterator mi;
54 for (mi = matrices.begin(); mi != matrices.end(); ++mi) {
55 nassertv(!(*mi).is_nan());
56 }
57 #endif // NDEBUG
58
59 // Get the greater of the local or global viscosity:
60 PN_stdfloat viscosityDamper=1.0f-physical->get_viscosity();
61
62 // Loop through each object in the set. This processing occurs in O(pf)
63 // time, where p is the number of physical objects and f is the number of
64 // forces. Unfortunately, no precomputation of forces can occur, as each
65 // force is possibly contingent on such things as the position and velocity
66 // of each physicsobject in the set. Accordingly, we have to grunt our way
67 // through each one. wrt caching of the xform matrix should help.
68 PhysicsObject::Vector::const_iterator current_object_iter;
69 current_object_iter = physical->get_object_vector().begin();
70 for (; current_object_iter != physical->get_object_vector().end();
71 ++current_object_iter) {
72 PhysicsObject *current_object = *current_object_iter;
73
74 // bail out if this object doesn't exist or doesn't want to be processed.
75 if (current_object == nullptr) {
76 continue;
77 }
78
79 if (current_object->get_active() == false) {
80 continue;
81 }
82
83 LVector3 md_accum_vec; // mass dependent accumulation vector.
84 LVector3 non_md_accum_vec;
85 LVector3 accel_vec;
86 LVector3 vel_vec;
87
88 // reset the accumulation vectors for this object
89 md_accum_vec.set(0.0f, 0.0f, 0.0f);
90 non_md_accum_vec.set(0.0f, 0.0f, 0.0f);
91
92 // run through each acting force and sum it
93 LVector3 f;
94 // LMatrix4 force_to_object_xform;
95
96 LinearForceVector::const_iterator f_cur;
97
98 // global forces
99 f_cur = forces.begin();
100 int index = 0;
101 for (; f_cur != forces.end(); ++f_cur) {
102 LinearForce *cur_force = *f_cur;
103
104 // make sure the force is turned on.
105 if (cur_force->get_active() == false) {
106 continue;
107 }
108
109 // now we go from force space to our object's space.
110 f = cur_force->get_vector(current_object) * matrices[index++];
111
112 physics_spam("child_integrate "<<f);
113 // tally it into the accum vectors.
114 if (cur_force->get_mass_dependent() == true) {
115 md_accum_vec += f;
116 } else {
117 non_md_accum_vec += f;
118 }
119 }
120
121 // local forces
122 f_cur = physical->get_linear_forces().begin();
123 for (; f_cur != physical->get_linear_forces().end(); ++f_cur) {
124 LinearForce *cur_force = *f_cur;
125
126 // make sure the force is turned on.
127 if (cur_force->get_active() == false) {
128 continue;
129 }
130
131 // go from force space to object space
132 f = cur_force->get_vector(current_object) * matrices[index++];
133
134 physics_spam("child_integrate "<<f);
135 // tally it into the accum vectors
136 if (cur_force->get_mass_dependent() == true) {
137 md_accum_vec += f;
138 } else {
139 non_md_accum_vec += f;
140 }
141 }
142
143 // get this object's physical info
144 LPoint3 pos = current_object->get_position();
145 vel_vec = current_object->get_velocity();
146 PN_stdfloat mass = current_object->get_mass();
147
148 // we want 'a' in F = ma get it by computing F m
149 nassertv(mass != 0.0f);
150 accel_vec = md_accum_vec / mass;
151 accel_vec += non_md_accum_vec;
152
153 #if 0 //[
154 // step the position and velocity
155 vel_vec += accel_vec * dt;
156
157 // cap terminal velocity
158 PN_stdfloat len = vel_vec.length();
159
160 if (len > current_object->get_terminal_velocity()) {
161 // cout << "Capping terminal velocity at: " <<
162 // current_object->get_terminal_velocity() << endl;
163 vel_vec *= current_object->get_terminal_velocity() / len;
164 }
165
166 pos += vel_vec * dt;
167 #else //][
168 assert(current_object->get_position()==current_object->get_last_position());
169
170 accel_vec*=viscosityDamper;
171
172 // x = x + v * t + 0.5 * a * t * t
173 pos += vel_vec * dt + 0.5 * accel_vec * dt * dt;
174 // v = v + a * t
175 vel_vec += accel_vec * dt;
176 #endif //]
177
178 // and store them back.
179 if (!pos.is_nan()) {
180 current_object->set_position(pos);
181 }
182 if (!vel_vec.is_nan()) {
183 current_object->set_velocity(vel_vec);
184 }
185 }
186 }
187
188 /**
189 * Write a string representation of this instance to <out>.
190 */
191 void LinearEulerIntegrator::
192 output(std::ostream &out) const {
193 #ifndef NDEBUG //[
194 out<<"LinearEulerIntegrator";
195 #endif //] NDEBUG
196 }
197
198 /**
199 * Write a string representation of this instance to <out>.
200 */
201 void LinearEulerIntegrator::
202 write(std::ostream &out, int indent) const {
203 #ifndef NDEBUG //[
204 out.width(indent);
205 out<<""<<"LinearEulerIntegrator:\n";
206 LinearIntegrator::write(out, indent+2);
207 #endif //] NDEBUG
208 }
LinearEulerIntegrator::output
virtual void output(std::ostream &out) const
Write a string representation of this instance to <out>.
Definition: linearEulerIntegrator.cxx:192
LinearEulerIntegrator::~LinearEulerIntegrator
virtual ~LinearEulerIntegrator()
destructor
Definition: linearEulerIntegrator.cxx:30
LinearEulerIntegrator::LinearEulerIntegrator
LinearEulerIntegrator()
constructor
Definition: linearEulerIntegrator.cxx:23
LinearEulerIntegrator::write
virtual void write(std::ostream &out, int indent=0) const
Write a string representation of this instance to <out>.
Definition: linearEulerIntegrator.cxx:202
LinearForce
A force that acts on a PhysicsObject by way of an Integrator.
Definition: linearForce.h:23
LinearIntegrator::write
virtual void write(std::ostream &out, int indent=0) const
Write a string representation of this instance to <out>.
Definition: linearIntegrator.cxx:81
Physical
Defines a set of physically modeled attributes.
Definition: physical.h:37
Physical::get_viscosity
get_viscosity
Get the local viscosity.
Definition: physical.h:75
PhysicsObject
A body on which physics will be applied.
Definition: physicsObject.h:27
PhysicsObject::get_active
get_active
Process Flag Query.
Definition: physicsObject.h:103
PhysicsObject::get_position
get_position
Position Query.
Definition: physicsObject.h:105
PhysicsObject::get_velocity
get_velocity
Velocity Query per second.
Definition: physicsObject.h:107
PhysicsObject::get_last_position
get_last_position
Get the position of the physics object at the start of the most recent do_physics.
Definition: physicsObject.h:106
PhysicsObject::set_velocity
set_velocity
Vector velocity assignment.
Definition: physicsObject.h:107
PhysicsObject::get_terminal_velocity
get_terminal_velocity
tv query
Definition: physicsObject.h:109
PhysicsObject::set_position
set_position
Vector position assignment.
Definition: physicsObject.h:105
PhysicsObject::get_mass
get_mass
Get the mass in slugs (or kilograms).
Definition: physicsObject.h:104
config_physics.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
forceNode.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
indent
std::ostream & indent(std::ostream &out, int indent_level)
A handy function for doing text formatting.
Definition: indent.cxx:20
linearEulerIntegrator.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
physicalNode.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.