1 /*
2 Copyright (C) 2005 by Ruben Henner Zilibowitz <rzilibowitz@users.sourceforge.net>
3 Part of the Toy Cars Project http://toycars.sourceforge.net
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the license.
7 This program is distributed in the hope that it will be useful,
8 but WITHOUT ANY WARRANTY.
9
10 See the COPYING file for more details.
11 */
12
13 /*
14 * Created by Ruben on Wed Apr 21 2004.
15 */
16
17 #include "RigidBody.h"
18 #include "Contact.h"
19
20 const double kTwoPi = 2 * M_PI;
21
RigidBody()22 RigidBody::RigidBody()
23 : mMass(HUGE_VAL),
24 mInertia(HUGE_VAL),
25
26 mInvMass(0),
27 mInvInertia(0),
28
29 mTotalForce_wc(Tuple(0,0)),
30 mTotalTorque(0),
31
32 mLocation_wc(Tuple(0,0)),
33 mVelocity_wc(Tuple(0,0)),
34 mAcceleration_wc(Tuple(0,0)),
35
36 mOrientation_wc(0),
37 mAngularVelocity(0),
38 mAngularAcceleration(0),
39
40 mNorm(-sin(0.0), cos(0.0)),
41
42 mGeom(Geom()),
43 mGeom_wc(Geom()),
44 mGeomRadius(0.0)
45 {
46 }
47
RigidBody(double inMass,double inInertia,Tuple inLocation_wc,Tuple inVelocity_wc,double inOrientation_wc,double inAngularVelocity,const Geom & inGeom)48 RigidBody::RigidBody( double inMass,
49 double inInertia,
50 Tuple inLocation_wc,
51 Tuple inVelocity_wc,
52 double inOrientation_wc,
53 double inAngularVelocity,
54 const Geom& inGeom)
55 : mMass(inMass),
56 mInertia(inInertia),
57
58 mInvMass(inMass > 1e6 ? 0 : 1/inMass),
59 mInvInertia(inInertia > 1e6 ? 0 : 1/inInertia),
60
61 mTotalForce_wc(Tuple(0,0)),
62 mTotalTorque(0),
63
64 mLocation_wc(inLocation_wc),
65 mVelocity_wc(inVelocity_wc),
66 mAcceleration_wc(Tuple(0,0)),
67
68 mOrientation_wc(inOrientation_wc),
69 mAngularVelocity(inAngularVelocity),
70 mAngularAcceleration(0),
71
72 mNorm(-sin(inOrientation_wc), cos(inOrientation_wc)),
73
74 mGeom(inGeom),
75 mGeom_wc(inGeom),
76 mGeomRadius(0.0)
77 {
78 mGeom_wc.setParent(this);
79 compute_geom_world_coords();
80
81 while (mOrientation_wc < 0)
82 mOrientation_wc += kTwoPi;
83 while (mOrientation_wc >= kTwoPi)
84 mOrientation_wc -= kTwoPi;
85
86 // compute geom radius
87 Geom::const_iterator a;
88 Convex::const_iterator i;
89 double r;
90 for (a = mGeom.begin(); a != mGeom.end(); a++)
91 {
92 for (i = a->begin(); i != a->end(); i++)
93 {
94 r = i->Length();
95 if (r > mGeomRadius)
96 mGeomRadius = r;
97 }
98 }
99 }
100
compute_force_and_torque()101 void RigidBody::compute_force_and_torque()
102 {
103 // default - simulation of gravity
104 // bodies with high mass are considered "anchored" and will not move
105
106 mTotalForce_wc.x = 0;
107
108 if (mInvMass > 0)
109 mTotalForce_wc.y = -9.8;
110 else
111 mTotalForce_wc.y = 0;
112
113 mTotalTorque = 0;
114 }
115
116 // integrate velocity and acceleration - method used is 2nd order Runge-Kutta
rk2_step(double dt)117 void RigidBody::rk2_step(double dt)
118 {
119 Tuple saveLoc = mLocation_wc;
120 double saveOrient = mOrientation_wc;
121 Tuple saveVel = mVelocity_wc;
122 double saveAngVel = mAngularVelocity;
123
124 // use half step
125
126 // integrate to get P'(t + 0.5)
127 mLocation_wc += 0.5 * dt * mVelocity_wc;
128 mOrientation_wc += 0.5 * dt * mAngularVelocity;
129
130 // integrate to get P(t + 0.5)
131 mVelocity_wc += 0.5 * dt * mAcceleration_wc;
132 mAngularVelocity += 0.5 * dt * mAngularAcceleration;
133
134 // bound the angle to 0..2pi
135 if (mOrientation_wc < 0)
136 mOrientation_wc += kTwoPi;
137 else if (mOrientation_wc >= kTwoPi)
138 mOrientation_wc -= kTwoPi;
139
140 // compute sine and cosine
141 mNorm.x = -sin(mOrientation_wc);
142 mNorm.y = cos(mOrientation_wc);
143
144 // compute P''(t + 0.5)
145 compute_force_and_torque();
146 mAcceleration_wc = mInvMass * mTotalForce_wc;
147 mAngularAcceleration = mInvInertia * mTotalTorque;
148
149 // use full step with half way derivatives
150
151 // integrate to get P'(t + 1)
152 mLocation_wc = saveLoc + (dt * mVelocity_wc);
153 mOrientation_wc = saveOrient + (dt * mAngularVelocity);
154
155 // integrate to get P(t + 1)
156 mVelocity_wc = saveVel + (dt * mAcceleration_wc);
157 mAngularVelocity = saveAngVel + (dt * mAngularAcceleration);
158
159 // bound the angle to 0..2pi
160 if (mOrientation_wc < 0)
161 mOrientation_wc += kTwoPi;
162 else if (mOrientation_wc >= kTwoPi)
163 mOrientation_wc -= kTwoPi;
164
165 // compute sine and cosine
166 mNorm.x = -sin(mOrientation_wc);
167 mNorm.y = cos(mOrientation_wc);
168
169 // compute P''(t + 1)
170 compute_force_and_torque();
171 mAcceleration_wc = mInvMass * mTotalForce_wc;
172 mAngularAcceleration = mInvInertia * mTotalTorque;
173 }
174
compute_geom_world_coords()175 void RigidBody::compute_geom_world_coords()
176 {
177 Geom::const_iterator a;
178 Geom::iterator b;
179 Convex::const_iterator i;
180 Convex::iterator j;
181
182 for (a = mGeom.begin(), b = mGeom_wc.begin(); a != mGeom.end(); a++, b++)
183 {
184 for (i = a->begin(), j = b->begin(); i != a->end(); i++, j++)
185 {
186 *j = mNorm.fromLocal(*i) + mLocation_wc;
187 }
188 }
189 }
190