1 // Copyright © 2008-2021 Pioneer Developers. See AUTHORS.txt for details
2 // Licensed under the terms of the GPL v3. See licenses/GPL-3.txt
3
4 #include "DynamicBody.h"
5
6 #include "FixedGuns.h"
7 #include "Frame.h"
8 #include "GameSaveError.h"
9 #include "Json.h"
10 #include "Planet.h"
11 #include "Space.h"
12 #include "collider/CollisionContact.h"
13 #include "ship/Propulsion.h"
14
15 static const float KINETIC_ENERGY_MULT = 0.00001f;
16 const double DynamicBody::DEFAULT_DRAG_COEFF = 0.1; // 'smooth sphere'
17
DynamicBody()18 DynamicBody::DynamicBody() :
19 ModelBody(),
20 m_propulsion(nullptr),
21 m_fixedGuns(nullptr)
22 {
23 m_dragCoeff = DEFAULT_DRAG_COEFF;
24 m_flags = Body::FLAG_CAN_MOVE_FRAME;
25 m_oldPos = GetPosition();
26 m_oldAngDisplacement = vector3d(0.0);
27 m_force = vector3d(0.0);
28 m_torque = vector3d(0.0);
29 m_vel = vector3d(0.0);
30 m_angVel = vector3d(0.0);
31 m_mass = 1;
32 m_angInertia = 1;
33 m_massRadius = 1;
34 m_isMoving = true;
35 m_atmosForce = vector3d(0.0);
36 m_gravityForce = vector3d(0.0);
37 m_externalForce = vector3d(0.0); // do external forces calc instead?
38 m_lastForce = vector3d(0.0);
39 m_lastTorque = vector3d(0.0);
40 m_aiMessage = AIError::AIERROR_NONE;
41 m_decelerating = false;
42 for (int i = 0; i < Feature::MAX_FEATURE; i++)
43 m_features[i] = false;
44 }
45
DynamicBody(const Json & jsonObj,Space * space)46 DynamicBody::DynamicBody(const Json &jsonObj, Space *space) :
47 ModelBody(jsonObj, space),
48 m_dragCoeff(DEFAULT_DRAG_COEFF),
49 m_externalForce(vector3d(0.0)),
50 m_atmosForce(vector3d(0.0)),
51 m_gravityForce(vector3d(0.0)),
52 m_lastForce(vector3d(0.0)),
53 m_lastTorque(vector3d(0.0)),
54 m_propulsion(nullptr),
55 m_fixedGuns(nullptr)
56 {
57 m_flags = Body::FLAG_CAN_MOVE_FRAME;
58 m_oldPos = GetPosition();
59 m_oldAngDisplacement = vector3d(0.0);
60
61 try {
62 Json dynamicBodyObj = jsonObj["dynamic_body"];
63
64 m_force = dynamicBodyObj["force"];
65 m_torque = dynamicBodyObj["torque"];
66 m_vel = dynamicBodyObj["vel"];
67 m_angVel = dynamicBodyObj["ang_vel"];
68 m_mass = dynamicBodyObj["mass"];
69 m_massRadius = dynamicBodyObj["mass_radius"];
70 m_angInertia = dynamicBodyObj["ang_inertia"];
71 m_isMoving = dynamicBodyObj["is_moving"];
72 } catch (Json::type_error &) {
73 throw SavedGameCorruptException();
74 }
75
76 m_aiMessage = AIError::AIERROR_NONE;
77 m_decelerating = false;
78 for (int i = 0; i < Feature::MAX_FEATURE; i++)
79 m_features[i] = false;
80 }
81
SaveToJson(Json & jsonObj,Space * space)82 void DynamicBody::SaveToJson(Json &jsonObj, Space *space)
83 {
84 ModelBody::SaveToJson(jsonObj, space);
85
86 Json dynamicBodyObj = Json::object(); // Create JSON object to contain dynamic body data.
87
88 dynamicBodyObj["force"] = m_force;
89 dynamicBodyObj["torque"] = m_torque;
90 dynamicBodyObj["vel"] = m_vel;
91 dynamicBodyObj["ang_vel"] = m_angVel;
92 dynamicBodyObj["mass"] = m_mass;
93 dynamicBodyObj["mass_radius"] = m_massRadius;
94 dynamicBodyObj["ang_inertia"] = m_angInertia;
95 dynamicBodyObj["is_moving"] = m_isMoving;
96
97 jsonObj["dynamic_body"] = dynamicBodyObj; // Add dynamic body object to supplied object.
98 }
99
GetCurrentAtmosphericState(double & pressure,double & density) const100 void DynamicBody::GetCurrentAtmosphericState(double &pressure, double &density) const
101 {
102 Frame *f = Frame::GetFrame(GetFrame());
103 Body *body = f->GetBody();
104 if (!body || !f->IsRotFrame() || !body->IsType(ObjectType::PLANET)) {
105 pressure = density = 0;
106 return;
107 }
108 Planet *planet = static_cast<Planet *>(body);
109 planet->GetAtmosphericState(GetPosition().Length(), &pressure, &density);
110 }
111
PostLoadFixup(Space * space)112 void DynamicBody::PostLoadFixup(Space *space)
113 {
114 Body::PostLoadFixup(space);
115 m_oldPos = GetPosition();
116 // CalcExternalForce(); // too dangerous
117 }
118
~DynamicBody()119 DynamicBody::~DynamicBody()
120 {
121 m_propulsion.Reset();
122 m_fixedGuns.Reset();
123 }
124
AddFeature(Feature f)125 void DynamicBody::AddFeature(Feature f)
126 {
127 m_features[f] = true;
128 if (f == Feature::PROPULSION && m_propulsion == nullptr) {
129 m_propulsion.Reset(new Propulsion());
130 } else if (f == Feature::FIXED_GUNS && m_fixedGuns == nullptr) {
131 m_fixedGuns.Reset(new FixedGuns());
132 }
133 }
134
SetForce(const vector3d & f)135 void DynamicBody::SetForce(const vector3d &f)
136 {
137 m_force = f;
138 }
139
AddForce(const vector3d & f)140 void DynamicBody::AddForce(const vector3d &f)
141 {
142 m_force += f;
143 }
144
AddTorque(const vector3d & t)145 void DynamicBody::AddTorque(const vector3d &t)
146 {
147 m_torque += t;
148 }
149
AddRelForce(const vector3d & f)150 void DynamicBody::AddRelForce(const vector3d &f)
151 {
152 m_force += GetOrient() * f;
153 }
154
AddRelTorque(const vector3d & t)155 void DynamicBody::AddRelTorque(const vector3d &t)
156 {
157 m_torque += GetOrient() * t;
158 }
159
GetPropulsion() const160 const Propulsion *DynamicBody::GetPropulsion() const
161 {
162 assert(m_propulsion != nullptr);
163 return m_propulsion.Get();
164 }
165
GetPropulsion()166 Propulsion *DynamicBody::GetPropulsion()
167 {
168 assert(m_propulsion != nullptr);
169 return m_propulsion.Get();
170 }
171
GetFixedGuns() const172 const FixedGuns *DynamicBody::GetFixedGuns() const
173 {
174 assert(m_fixedGuns != nullptr);
175 return m_fixedGuns.Get();
176 }
177
GetFixedGuns()178 FixedGuns *DynamicBody::GetFixedGuns()
179 {
180 assert(m_fixedGuns != nullptr);
181 return m_fixedGuns.Get();
182 }
183
SetTorque(const vector3d & t)184 void DynamicBody::SetTorque(const vector3d &t)
185 {
186 m_torque = t;
187 }
188
SetMass(double mass)189 void DynamicBody::SetMass(double mass)
190 {
191 m_mass = mass;
192 // This is solid sphere mass distribution, my friend
193 m_angInertia = (2 / 5.0) * m_mass * m_massRadius * m_massRadius;
194 }
195
SetFrame(FrameId fId)196 void DynamicBody::SetFrame(FrameId fId)
197 {
198 ModelBody::SetFrame(fId);
199 // external forces will be wrong after frame transition
200 m_externalForce = m_gravityForce = m_atmosForce = vector3d(0.0);
201 }
202
CalcAtmosphericDrag(double velSqr,double area,double coeff) const203 double DynamicBody::CalcAtmosphericDrag(double velSqr, double area, double coeff) const
204 {
205 double pressure, density;
206 GetCurrentAtmosphericState(pressure, density);
207
208 // Simplified calculation of atmospheric drag/lift force.
209 return density > 0 ? 0.5 * density * velSqr * area * coeff : 0;
210 }
211
CalcAtmosphericForce() const212 vector3d DynamicBody::CalcAtmosphericForce() const
213 {
214 vector3d dragDir = -m_vel.NormalizedSafe();
215
216 // We assume the object is a perfect sphere in the size of the clip radius.
217 // Most things are /not/ using the default DynamicBody code, but this is still better than before.
218 return CalcAtmosphericDrag(m_vel.LengthSqr(), GetClipRadius() * GetClipRadius() * M_PI, m_dragCoeff) * dragDir;
219 }
220
CalcExternalForce()221 void DynamicBody::CalcExternalForce()
222 {
223 // gravity
224 Frame *f = Frame::GetFrame(GetFrame());
225 if (!f) return; // no external force if not in a frame
226 Body *body = f->GetBody();
227 if (body && !body->IsType(ObjectType::SPACESTATION)) { // they ought to have mass though...
228 vector3d b1b2 = GetPosition();
229 double m1m2 = GetMass() * body->GetMass();
230 double invrsqr = 1.0 / b1b2.LengthSqr();
231 double force = G * m1m2 * invrsqr;
232 m_externalForce = -b1b2 * sqrt(invrsqr) * force;
233 } else
234 m_externalForce = vector3d(0.0);
235 m_gravityForce = m_externalForce;
236
237 // atmospheric drag
238 if (body && f->IsRotFrame() && body->IsType(ObjectType::PLANET)) {
239 vector3d fAtmoForce = CalcAtmosphericForce();
240
241 // make this a bit less daft at high time accel
242 // only allow atmosForce to increase by .1g per frame
243 // TODO: clamp fAtmoForce instead.
244 vector3d f1g = m_atmosForce + fAtmoForce.NormalizedSafe() * GetMass();
245 if (fAtmoForce.LengthSqr() > f1g.LengthSqr())
246 m_atmosForce = f1g;
247 else
248 m_atmosForce = fAtmoForce;
249
250 m_externalForce += m_atmosForce;
251 } else
252 m_atmosForce = vector3d(0.0);
253
254 // centrifugal and coriolis forces for rotating frames
255 if (f->IsRotFrame()) {
256 vector3d angRot(0, f->GetAngSpeed(), 0);
257 m_externalForce -= m_mass * angRot.Cross(angRot.Cross(GetPosition())); // centrifugal
258 m_externalForce -= 2 * m_mass * angRot.Cross(GetVelocity()); // coriolis
259 }
260 }
261
TimeStepUpdate(const float timeStep)262 void DynamicBody::TimeStepUpdate(const float timeStep)
263 {
264 m_oldPos = GetPosition();
265 if (m_isMoving) {
266 m_force += m_externalForce;
267
268 m_vel += double(timeStep) * m_force * (1.0 / m_mass);
269 m_angVel += double(timeStep) * m_torque * (1.0 / m_angInertia);
270
271 double len = m_angVel.Length();
272 if (len > 1e-16) {
273 vector3d axis = m_angVel * (1.0 / len);
274 matrix3x3d r = matrix3x3d::Rotate(len * timeStep, axis);
275 SetOrient(r * GetOrient());
276 }
277 m_oldAngDisplacement = m_angVel * timeStep;
278
279 SetPosition(GetPosition() + m_vel * double(timeStep));
280
281 //if (this->IsType(ObjectType::PLAYER))
282 //Output("pos = %.1f,%.1f,%.1f, vel = %.1f,%.1f,%.1f, force = %.1f,%.1f,%.1f, external = %.1f,%.1f,%.1f\n",
283 // pos.x, pos.y, pos.z, m_vel.x, m_vel.y, m_vel.z, m_force.x, m_force.y, m_force.z,
284 // m_externalForce.x, m_externalForce.y, m_externalForce.z);
285
286 m_lastForce = m_force;
287 m_lastTorque = m_torque;
288 m_force = vector3d(0.0);
289 m_torque = vector3d(0.0);
290 CalcExternalForce(); // regenerate for new pos/vel
291 } else {
292 m_oldAngDisplacement = vector3d(0.0);
293 }
294
295 ModelBody::TimeStepUpdate(timeStep);
296 }
297
UpdateInterpTransform(double alpha)298 void DynamicBody::UpdateInterpTransform(double alpha)
299 {
300 m_interpPos = alpha * GetPosition() + (1.0 - alpha) * m_oldPos;
301
302 double len = m_oldAngDisplacement.Length() * (1.0 - alpha);
303 if (len > 1e-16) {
304 vector3d axis = m_oldAngDisplacement.Normalized();
305 matrix3x3d rot = matrix3x3d::Rotate(-len, axis); // rotate backwards
306 m_interpOrient = rot * GetOrient();
307 } else
308 m_interpOrient = GetOrient();
309 }
310
SetMassDistributionFromModel()311 void DynamicBody::SetMassDistributionFromModel()
312 {
313 CollMesh *m = GetCollMesh();
314 // XXX totally arbitrarily pick to distribute mass over a half
315 // bounding sphere area
316 m_massRadius = m->GetRadius() * 0.5f;
317 SetMass(m_mass);
318 }
319
GetAngularMomentum() const320 vector3d DynamicBody::GetAngularMomentum() const
321 {
322 return m_angInertia * m_angVel;
323 }
324
GetVelocity() const325 vector3d DynamicBody::GetVelocity() const
326 {
327 return m_vel;
328 }
329
SetVelocity(const vector3d & v)330 void DynamicBody::SetVelocity(const vector3d &v)
331 {
332 m_vel = v;
333 }
334
GetAngVelocity() const335 vector3d DynamicBody::GetAngVelocity() const
336 {
337 return m_angVel;
338 }
339
SetAngVelocity(const vector3d & v)340 void DynamicBody::SetAngVelocity(const vector3d &v)
341 {
342 m_angVel = v;
343 }
344
OnCollision(Body * o,Uint32 flags,double relVel)345 bool DynamicBody::OnCollision(Body *o, Uint32 flags, double relVel)
346 {
347 // don't bother doing collision damage from a missile that will now explode, or may have already
348 // also avoids an occasional race condition where destruction event of this could be queued twice
349 // returning true to ensure that the missile can react to the collision
350 if (o->IsType(ObjectType::MISSILE)) return true;
351
352 double kineticEnergy = 0;
353 if (o->IsType(ObjectType::DYNAMICBODY)) {
354 kineticEnergy = KINETIC_ENERGY_MULT * static_cast<DynamicBody *>(o)->GetMass() * relVel * relVel;
355 } else {
356 kineticEnergy = KINETIC_ENERGY_MULT * m_mass * relVel * relVel;
357 }
358
359 // damage (kineticEnergy is being passed as a damage value) is measured in kilograms
360 // ignore damage less than a gram except for cargo, which is very fragile.
361 CollisionContact dummy;
362 if (this->IsType(ObjectType::CARGOBODY)) {
363 OnDamage(o, float(kineticEnergy), dummy);
364 } else if (kineticEnergy > 1e-3) {
365 OnDamage(o, float(kineticEnergy), dummy);
366 }
367
368 return true;
369 }
370
371 // return parameters for orbit of any body, gives both elliptic and hyperbolic trajectories
ComputeOrbit() const372 Orbit DynamicBody::ComputeOrbit() const
373 {
374 auto f = Frame::GetFrame(GetFrame());
375 // if we are in a rotating frame, then dynamic body currently under the
376 // influence of a rotational frame, therefore getting the orbital parameters
377 // is not appropriate, return the orbit as a fixed point
378 if (f->IsRotFrame()) return Orbit::ForStaticBody(GetPosition());
379 FrameId nrFrameId = f->GetId();
380 const Frame *nrFrame = Frame::GetFrame(nrFrameId);
381 const double mass = nrFrame->GetSystemBody()->GetMass();
382
383 // current velocity and position with respect to non-rotating frame
384 const vector3d vel = GetVelocityRelTo(nrFrameId);
385 const vector3d pos = GetPositionRelTo(nrFrameId);
386
387 return Orbit::FromBodyState(pos, vel, mass);
388 }
389