Panda3D

angularEulerIntegrator.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 angularEulerIntegrator.cxx
10 * @author charles
11 * @date 2000-08-09
12 */
13
14 #include "angularEulerIntegrator.h"
15 #include "forceNode.h"
16 #include "physicalNode.h"
17 #include "config_physics.h"
18
19 /**
20 * constructor
21 */
22 AngularEulerIntegrator::
23 AngularEulerIntegrator() {
24 }
25
26 /**
27 * destructor
28 */
29 AngularEulerIntegrator::
30 ~AngularEulerIntegrator() {
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 void AngularEulerIntegrator::
38 child_integrate(Physical *physical,
39 AngularForceVector& forces,
40 PN_stdfloat dt) {
41 // Loop through each object in the set. This processing occurs in O(pf)
42 // time, where p is the number of physical objects and f is the number of
43 // forces. Unfortunately, no precomputation of forces can occur, as each
44 // force is possibly contingent on such things as the position and velocity
45 // of each physicsobject in the set. Accordingly, we have to grunt our way
46 // through each one. wrt caching of the xform matrix should help.
47 PhysicsObject::Vector::const_iterator current_object_iter;
48 current_object_iter = physical->get_object_vector().begin();
49 for (; current_object_iter != physical->get_object_vector().end();
50 ++current_object_iter) {
51 PhysicsObject *current_object = *current_object_iter;
52
53 // bail out if this object doesn't exist or doesn't want to be processed.
54 if (current_object == nullptr) {
55 continue;
56 }
57
58 if (current_object->get_active() == false) {
59 continue;
60 }
61
62 LRotation accum_quat(0, 0, 0, 0);
63
64 // set up the traversal stuff.
65 AngularForceVector::const_iterator f_cur;
66
67 LRotation f;
68
69 // global forces
70 f_cur = forces.begin();
71 // unsigned int index = 0;
72 for (; f_cur != forces.end(); ++f_cur) {
73 AngularForce *cur_force = *f_cur;
74
75 // make sure the force is turned on.
76 if (cur_force->get_active() == false) {
77 continue;
78 }
79
80 // tally it into the accumulation quaternion
81 f = cur_force->get_quat(current_object);
82 accum_quat += f;
83 }
84
85 LOrientation orientation = current_object->get_orientation();
86 // local forces
87 f_cur = physical->get_angular_forces().begin();
88 for (; f_cur != physical->get_angular_forces().end(); ++f_cur) {
89 AngularForce *cur_force = *f_cur;
90
91 // make sure the force is turned on.
92 if (cur_force->get_active() == false) {
93 continue;
94 }
95
96 f = cur_force->get_quat(current_object);
97
98 // tally it into the accumulation quaternion i.e. orientation * f *
99 // orientation.conjugate()
100 accum_quat += orientation.xform(f);
101 }
102
103 // apply the accumulated torque vector to the object's inertial tensor.
104 // this matrix represents how much force the object 'wants' applied to it
105 // in any direction, among other things.
106 accum_quat = current_object->get_inertial_tensor() * accum_quat;
107
108 // derive this into the angular velocity vector.
109 LRotation rot_quat = current_object->get_rotation();
110 #if 0
111 rot_quat += accum_quat * dt;
112
113 if (rot_quat.normalize()) {
114 LOrientation old_orientation = current_object->get_orientation();
115 LOrientation new_orientation = old_orientation * rot_quat;
116 new_orientation.normalize();
117
118 // and write the results back.
119 current_object->set_orientation(new_orientation);
120 current_object->set_rotation(rot_quat);
121 }
122 #else
123 // accum_quat*=viscosityDamper; LOrientation orientation =
124 // current_object->get_orientation();
125
126 // accum_quat.normalize(); x = x + v * t + 0.5 * a * t * t
127 orientation = orientation * ((rot_quat * dt) * (accum_quat * (0.5 * dt * dt)));
128 // v = v + a * t
129 rot_quat = rot_quat + (accum_quat * dt);
130
131 // if (rot_quat.normalize()) {
132 if (orientation.normalize() && rot_quat.normalize()) {
133 // and write the results back.
134 current_object->set_orientation(orientation);
135 current_object->set_rotation(rot_quat);
136 }
137 #endif
138 }
139 }
140
141 /**
142 * Write a string representation of this instance to <out>.
143 */
144 void AngularEulerIntegrator::
145 output(std::ostream &out) const {
146 #ifndef NDEBUG //[
147 out<<"AngularEulerIntegrator (id "<<this<<")";
148 #endif //] NDEBUG
149 }
150
151 /**
152 * Write a string representation of this instance to <out>.
153 */
154 void AngularEulerIntegrator::
155 write(std::ostream &out, int indent) const {
156 #ifndef NDEBUG //[
157 out.width(indent); out<<""; out<<"AngularEulerIntegrator:\n";
158 AngularIntegrator::write(out, indent+2);
159 #endif //] NDEBUG
160 }
angularEulerIntegrator.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
AngularEulerIntegrator::write
virtual void write(std::ostream &out, int indent=0) const
Write a string representation of this instance to <out>.
Definition: angularEulerIntegrator.cxx:155
AngularEulerIntegrator::output
virtual void output(std::ostream &out) const
Write a string representation of this instance to <out>.
Definition: angularEulerIntegrator.cxx:145
AngularEulerIntegrator::~AngularEulerIntegrator
virtual ~AngularEulerIntegrator()
destructor
Definition: angularEulerIntegrator.cxx:30
AngularEulerIntegrator::AngularEulerIntegrator
AngularEulerIntegrator()
constructor
Definition: angularEulerIntegrator.cxx:23
AngularForce
pure virtual parent of all quat-based forces.
Definition: angularForce.h:22
AngularForce::get_quat
LRotation get_quat(const PhysicsObject *po)
access query
Definition: angularForce.cxx:45
AngularIntegrator::write
virtual void write(std::ostream &out, int indent=0) const
Write a string representation of this instance to <out>.
Definition: angularIntegrator.cxx:62
Physical
Defines a set of physically modeled attributes.
Definition: physical.h:37
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_inertial_tensor
virtual LMatrix4 get_inertial_tensor() const
returns a transform matrix that represents the object's willingness to be forced.
Definition: physicsObject.cxx:145
PhysicsObject::set_rotation
set_rotation
set rotation as a quaternion delta per second.
Definition: physicsObject.h:112
PhysicsObject::get_rotation
get_rotation
get rotation per second.
Definition: physicsObject.h:112
PhysicsObject::get_orientation
get_orientation
get current orientation.
Definition: physicsObject.h:111
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
physicalNode.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.