1 /*************************************************************************
2 * *
3 * Tokamak Physics Engine, Copyright (C) 2002-2007 David Lam. *
4 * All rights reserved. Email: david@tokamakphysics.com *
5 * Web: www.tokamakphysics.com *
6 * *
7 * This library is distributed in the hope that it will be useful, *
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
10 * LICENSE.TXT for more details. *
11 * *
12 *************************************************************************/
13
14 #pragma inline_recursion( on )
15 #pragma inline_depth( 250 )
16
17 #include "stdio.h"
18
19 /*
20 #ifdef _WIN32
21 #include <windows.h>
22 #endif
23 */
24
25 #include "tokamak.h"
26 #include "containers.h"
27 #include "scenery.h"
28 #include "collision.h"
29 #include "constraint.h"
30 #include "rigidbody.h"
31 #include "scenery.h"
32 #include "stack.h"
33 #include "simulator.h"
34 #include "message.h"
35
36 /****************************************************************************
37 *
38 * neRigidBodyBase::GetConvex
39 *
40 ****************************************************************************/
41 /*
42 TConvex * neRigidBodyBase::GetConvex(s32 index)
43 {
44 ASSERT(index < col.convexCount);
45
46 if (col.convexCount == 0)
47 return NULL;
48
49 if (col.convexCount == 1 && index == 0)
50 return &col.obb;
51
52 if (index == -1)
53 {
54 return &col.obb;
55 }
56
57 return &col.convex[index];
58 }
59 */
CollideConnected(neBool yes)60 void neRigidBodyBase::CollideConnected(neBool yes)
61 {
62 isCollideConnected = yes;
63 }
64
CollideConnected()65 neBool neRigidBodyBase::CollideConnected()
66 {
67 return isCollideConnected;
68 }
69
RecalcBB()70 void neRigidBodyBase::RecalcBB()
71 {
72 col.CalcBB();
73
74 neV3 maxExt, minExt;
75
76 col.obb.GetExtend(minExt, maxExt);
77
78 neSensorItem * si = (neSensorItem *)sensors;
79
80 while (si)
81 {
82 neSensor_ * sensor = (neSensor_ *) si;
83
84 si = si->next;
85
86 neV3 sstart;
87 neV3 send;
88
89 sstart = sensor->pos;
90 send = sensor->pos + sensor->dir;
91
92 for (s32 j = 0; j < 3; j++)
93 {
94 maxExt[j] = neMax(maxExt[j], sstart[j]);
95 maxExt[j] = neMax(maxExt[j], send[j]);
96 minExt[j] = neMin(minExt[j], sstart[j]);
97 minExt[j] = neMin(minExt[j], send[j]);
98 }
99 }
100
101 for (s32 i = 0; i < 3; i++)
102 {
103 col.obb.as.box.boxSize[i] = ( maxExt[i] - minExt[i] ) * 0.5f;
104 col.obb.c2p.pos[i] = minExt[i] + col.obb.as.box.boxSize[i];
105 }
106
107 obb.rot[0] = col.obb.as.box.boxSize[0] * col.obb.c2p.rot[0];
108 obb.rot[1] = col.obb.as.box.boxSize[1] * col.obb.c2p.rot[1];
109 obb.rot[2] = col.obb.as.box.boxSize[2] * col.obb.c2p.rot[2];
110 obb.pos = col.obb.c2p.pos;
111
112 };
113
114 /*
115 neV3 neRigidBodyBase::VelocityAtPoint(const neV3 & pt)
116 {
117 neV3 ret;
118
119 if (AsCollisionBody())
120 {
121 ret.SetZero();
122
123 return ret;
124 }
125 else
126 {
127 ret = ((neRigidBody_*)this)->Derive().linearVel;
128
129 ret += ((neRigidBody_*)this)->Derive().angularVel.Cross(pt);
130
131 return ret;
132 }
133 }
134 */
AddSensor()135 neSensor_ * neRigidBodyBase::AddSensor()
136 {
137 neSensor_ * newSensor = sim->sensorHeap.Alloc(1);
138
139 if (!newSensor)
140 {
141 sprintf(sim->logBuffer, MSG_RUN_OUT_SENSOR);
142
143 sim->LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
144
145 return NULL;
146 }
147 if (sensors)
148 {
149 //((neSensorItem *)sensors)->Append((neSensorItem *)newSensor);
150
151 neSensorItem * sitem = (neSensorItem *)sensors;
152
153 while (sitem->next)
154 {
155 sitem = sitem->next;
156 }
157 sitem->Append((neSensorItem *)newSensor);
158 }
159 else
160 {
161 sensors = newSensor;
162 }
163 return newSensor;
164 }
165
BeginIterateSensor()166 void neRigidBodyBase::BeginIterateSensor()
167 {
168 sensorCursor = (neSensorItem *)sensors;
169 }
170
GetNextSensor()171 neSensor_ * neRigidBodyBase::GetNextSensor()
172 {
173 if (!sensorCursor)
174 return NULL;
175
176 neSensor_ * ret = (neSensor_ *)sensorCursor;
177
178 sensorCursor = sensorCursor->next;
179
180 return ret;
181 }
182
ClearSensor()183 void neRigidBodyBase::ClearSensor()
184 {
185 neSensorItem * si = (neSensorItem *)sensors;
186
187 while (si)
188 {
189 neSensor_ * s = (neSensor_ *) si;
190
191 si = si->next;
192
193 s->depth = 0.0f;
194
195 s->body = NULL;
196 }
197 }
198
AddGeometry()199 TConvex * neRigidBodyBase::AddGeometry()
200 {
201 TConvex * newConvex = sim->geometryHeap.Alloc(1);
202
203 if (!newConvex)
204 {
205 sprintf(sim->logBuffer, MSG_RUN_OUT_GEOMETRY);
206
207 sim->LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
208
209 return NULL;
210 }
211 newConvex->Initialise();
212
213 if (col.convex)
214 {
215 TConvexItem * citem = (TConvexItem *)col.convex;
216
217 while (citem)
218 {
219 if (!citem->next)
220 {
221 citem->Append((TConvexItem *)newConvex);
222
223 break;
224 }
225 else
226 {
227 citem = citem->next;
228 }
229 }
230 }
231 else
232 {
233 col.convex = newConvex;
234 }
235 col.convexCount++;
236
237 if (isActive && !IsInRegion())
238 sim->region.AddBody(this, NULL);
239
240 return newConvex;
241 }
242
BeginIterateGeometry()243 void neRigidBodyBase::BeginIterateGeometry()
244 {
245 geometryCursor = (TConvexItem *)col.convex;
246 }
247
GetNextGeometry()248 TConvex * neRigidBodyBase::GetNextGeometry()
249 {
250 if (!geometryCursor)
251 return NULL;
252
253 TConvex * ret = (TConvex *)geometryCursor;
254
255 geometryCursor = geometryCursor->next;
256
257 return ret;
258 }
259
RemoveConstraintHeader()260 void neRigidBodyBase::RemoveConstraintHeader()
261 {
262 neConstraintHeader * h = GetConstraintHeader();
263
264 if (h)
265 {
266 h->bodies.Remove(&constraintHeaderItem);
267
268 h->flag = neConstraintHeader::FLAG_NEED_REORG;
269
270 SetConstraintHeader(NULL);
271
272 if (h->bodies.count == 0)
273 {
274 sim->constraintHeaders.Dealloc(h);
275 }
276 }
277 }
278
Free()279 void neRigidBodyBase::Free()
280 {
281 //free sensor
282
283 neFreeListItem<neSensor_> * si = (neFreeListItem<neSensor_> *) sensors;
284
285 while (si)
286 {
287 neFreeListItem<neSensor_> * next = si->next;
288
289 //si->Remove();
290
291 sim->sensorHeap.Dealloc((neSensor_*)si, 1);
292
293 si = next;
294 }
295 sensors = NULL;
296
297 //remove from region
298 if (IsInRegion())
299 sim->region.RemoveBody(this);
300
301 //free geometry
302
303 neFreeListItem<TConvex> * gi = (neFreeListItem<TConvex> *) col.convex;
304
305 while (gi)
306 {
307 neFreeListItem<TConvex> * next = gi->next;
308
309 //gi->Remove();
310
311 sim->geometryHeap.Dealloc((TConvex*)gi, 1);
312
313 gi = next;
314 }
315 col.convex = NULL;
316
317 col.convexCount = 0;
318
319 //free constraint
320 neConstraintHandle * chandle = constraintCollection.GetHead();
321
322 while (chandle)
323 {
324 _neConstraint * c = chandle->thing;
325
326 chandle = constraintCollection.GetNext(chandle);
327
328 c->bodyA->constraintCollection.Remove(&c->bodyAHandle);
329
330 if (c->bodyB)
331 c->bodyB->constraintCollection.Remove(&c->bodyBHandle);
332
333 if (GetConstraintHeader())
334 GetConstraintHeader()->Remove(c);
335
336 neFreeListItem<neController> * ci = (neFreeListItem<neController> *) c->controllers;
337
338 while (ci)
339 {
340 neFreeListItem<neController> * next = ci->next;
341
342 ci->Remove();
343
344 sim->controllerHeap.Dealloc((neController *)ci, 1);
345
346 ci = next;
347 }
348 c->controllers = NULL;
349
350 sim->constraintHeap.Dealloc(c, 1);
351 }
352 neRestRecordHandle * rhandle = rbRestingOnMe.GetHead();
353
354 while (rhandle)
355 {
356 neRestRecord * r = rhandle->thing;
357
358 rhandle = rbRestingOnMe.GetNext(rhandle);
359
360 r->SetInvalid();
361 };
362 }
363
IsValid()364 neBool neRigidBodyBase::IsValid()
365 {
366 if (btype == NE_OBJECT_COLISION)
367 {
368 return ((neList<neCollisionBody_>::itemType *)this)->state;// sim->abHeap.IsInUse((neCollisionBody_*)this);
369 }
370 else
371 {
372 return ((neList<neRigidBody_>::itemType *)this)->state;//sim->rbHeap.IsInUse((neRigidBody_*)this);
373 }
374 }
375
GetB2W()376 neT3 & neRigidBodyBase::GetB2W()
377 {
378 if (btype == NE_OBJECT_COLISION)
379 {
380 return AsCollisionBody()->b2w;
381 }
382 else
383 {
384 return AsRigidBody()->State().b2w;
385 }
386 }
387
DrawCPointLine()388 void neRigidBody_::DrawCPointLine()
389 {
390 return;
391
392 #if 0
393 neConstraintPointArray & pointArray = GetRBCData().GetCPointArray();
394
395 for (s32 i = 0; i < pointArray.GetUsedCount(); i++)
396 {
397 for (s32 j = i + 1; j < pointArray.GetUsedCount(); j++)
398 {
399 // if (pointArray[i].constraint == pointArray[j].constraint)
400 // continue;
401
402 neV3 points[2];
403 neV3 color;
404
405 points[0] = *pointArray[i].GetPtResult(this);
406 points[1] = *pointArray[j].GetPtResult(this);
407 f32 test = (points[0] - points[1]).Length();
408 DrawLine(color, points, 2);
409 }
410 }
411 if (calignMethod == ALIGN_POINT_ORIGIN ||
412 calignMethod == ALIGN_LINE_ORIGIN ||
413 calignMethod == ALIGN_TRI_AUX)
414 {
415 for (i = 0; i < pointArray.GetUsedCount(); i++)
416 {
417 neV3 points[2];
418 neV3 color;
419
420 points[0] = *pointArray[i].GetPtResult(this);
421 points[1] = auxCPoints[1];
422
423 f32 test = (points[0] - points[1]).Length();
424 DrawLine(color, points, 2);
425 }
426
427 }
428 #endif
429 }
430
Active(neBool yes,neRigidBodyBase * hint)431 void neRigidBodyBase::Active(neBool yes, neRigidBodyBase * hint)
432 {
433 if (isActive && yes)
434 return;
435
436 if (isActive) //make inactive
437 {
438 if (AsCollisionBody())
439 {
440 sim->activeCB.Remove((neCollisionBody_*)this);
441
442 sim->inactiveCB.Add((neCollisionBody_*)this);
443 }
444 else
445 {
446 if (AsRigidBody()->IsParticle())
447 {
448 sim->activeRP.Remove((neRigidBody_*)this);
449
450 sim->inactiveRP.Add((neRigidBody_*)this);
451 }
452 else
453 {
454 sim->activeRB.Remove((neRigidBody_*)this);
455
456 sim->inactiveRB.Add((neRigidBody_*)this);
457 }
458 }
459 //remove from region
460
461 if (IsInRegion())
462 sim->region.RemoveBody(this);
463
464 isActive = false;
465 }
466 else //make active
467 {
468 if (AsCollisionBody())
469 {
470 sim->inactiveCB.Remove((neCollisionBody_*)this);
471
472 sim->activeCB.Add((neCollisionBody_*)this);
473 }
474 else
475 {
476 if (AsRigidBody()->IsParticle())
477 {
478 sim->inactiveRP.Remove((neRigidBody_*)this);
479
480 sim->activeRP.Add((neRigidBody_*)this);
481 }
482 else
483 {
484 sim->inactiveRB.Remove((neRigidBody_*)this);
485
486 sim->activeRB.Add((neRigidBody_*)this);
487 }
488 }
489 //insert into the region
490
491 if (col.convexCount > 0 || isCustomCD)
492 sim->region.AddBody(this, hint);
493
494 isActive = true;
495 }
496 }
497