SO3Engine
OgreNewt_Body.cpp
Go to the documentation of this file.
6
7#include "SO3Utils/SO3Euler.h"
8
9
10namespace OgreNewt
11{
12
13 Body::Body(World* const W, const OgreNewt::CollisionPtr& col, int bodytype, bool trigger) :
14 m_world(W)
15 {
16 m_collision = col;
17 m_type = bodytype;
18 m_node = 0;
19 m_matid = 0;
20 m_userdata = 0;
21
22 m_triggerController = 0;
27 //m_transformcallback = 0;
30
31 //m_nodeupdateneeded = false;
32
33 m_nodePosit = Ogre::Vector3(0.0f, 0.0f, 0.0f);
34 m_curPosit = Ogre::Vector3(0.0f, 0.0f, 0.0f);
35 m_prevPosit = Ogre::Vector3(0.0f, 0.0f, 0.0f);
36 m_curRotation = Ogre::Quaternion::IDENTITY;
37 m_prevRotation = Ogre::Quaternion::IDENTITY;
38 m_nodeRotation = Ogre::Quaternion::IDENTITY;
39 m_scale = Ogre::Vector3::UNIT_SCALE;
40
41 //Newton 2.30
42 dFloat matrix[16];
44
46
47 if (trigger)
48 {
49 m_triggerController = m_world->CreateController(&matrix[0], col->getNewtonCollision());
50 WorldTriggerManager::TriggerCallback* const triggerCb = (WorldTriggerManager::TriggerCallback*) m_triggerController->GetUserData();
51 triggerCb->setTriggerEnterCallback<Body>(&Body::newtonTriggerEnterCallback, this);
52 triggerCb->setTriggerInsideCallback<Body>(&Body::newtonTriggerInsideCallback, this);
53 triggerCb->setTriggerExitCallback<Body>(&Body::newtonTriggerExitCallback, this);
54 m_body = m_triggerController->GetBody();
55 }
56 else
57 {
58 m_body = NewtonCreateDynamicBody(m_world->getNewtonWorld(), col->getNewtonCollision(), &matrix[0]);
59 }
60
61 if (m_body)
62 {
63 NewtonBodySetUserData(m_body, this);
64 NewtonBodySetDestructorCallback(m_body, newtonDestructor);
65 NewtonBodySetTransformCallback(m_body, newtonTransformCallback);
66
67 //setLinearDamping(m_world->getDefaultLinearDamping() * (60.0f / m_world->getUpdateFPS()));
68 //setAngularDamping(m_world->getDefaultAngularDamping() * (60.0f / m_world->getUpdateFPS()));
69 }
70 }
71
73 {
74 if (m_body)
75 {
83
84 NewtonBodySetUserData(m_body, NULL);
85 NewtonBodySetDestructorCallback(m_body, NULL);
86 NewtonBodySetForceAndTorqueCallback(m_body, NULL);
87 NewtonBodySetTransformCallback(m_body, NULL);
88 if (m_triggerController)
89 {
90 m_world->DestroyController(m_triggerController);
91 m_triggerController = 0;
92 }
93 else
94 {
95 NewtonDestroyBody(m_body);
96 }
97
98 m_body = 0;
99 }
100 }
101
102 // destructor callback
103 void _CDECL Body::newtonDestructor(const NewtonBody* body)
104 {
105 //newton wants to destroy the body.. so first find it.
106 OgreNewt::Body* me = static_cast<OgreNewt::Body*>(NewtonBodyGetUserData(body));
107 if (me)
108 {
109 // remove destructor callback
110 NewtonBodySetDestructorCallback(body, NULL);
111 // remove the user data
112 NewtonBodySetUserData(body, NULL);
113
114 NewtonBodySetForceAndTorqueCallback(body, NULL);
115 NewtonBodySetTransformCallback(body, NULL);
116
117 me->m_forcecallback = 0;
120 me->m_triggerExitCallback = 0;
123
124 me->m_body = 0;
125 }
126 }
127
128 // transform callback
129 void _CDECL Body::newtonTransformCallback(const NewtonBody* body, const dFloat* matrix, int threadIndex)
130 {
131 Ogre::Real dot;
132 OgreNewt::Body* me;
133
134 me = static_cast<OgreNewt::Body*>(NewtonBodyGetUserData(body));
135 if (me)
136 {
137 me->m_prevPosit = me->m_curPosit;
139 me->m_curPosit = Ogre::Vector3(Ogre::Real(matrix[12]), Ogre::Real(matrix[13]), Ogre::Real(matrix[14]));
140
141 // use the rotation from the body
142 dVector rot;
143 NewtonBodyGetRotation(body, &rot.m_x);
144 me->m_curRotation.w = Ogre::Real(rot.m_x);
145 me->m_curRotation.x = Ogre::Real(rot.m_y);
146 me->m_curRotation.y = Ogre::Real(rot.m_z);
147 me->m_curRotation.z = Ogre::Real(rot.m_w);
148
149 dot = me->m_prevRotation.Dot(me->m_curRotation);
150
151 if (dot < 0.0f)
152 me->m_prevRotation = -1.0f * me->m_prevRotation;
153 }
154 }
155
156 void Body::newtonTriggerEnterCallback(NewtonBody* const visitor)
157 {
158 OgreNewt::Body* me = static_cast<OgreNewt::Body*>(NewtonBodyGetUserData(visitor));
159 {
162 }
163 }
164
165 void Body::newtonTriggerInsideCallback(NewtonBody* const visitor)
166 {
167 OgreNewt::Body* me = static_cast<OgreNewt::Body*>(NewtonBodyGetUserData(visitor));
168 if (me)
169 {
172 }
173 }
174
175 void Body::newtonTriggerExitCallback(NewtonBody* const visitor)
176 {
177 OgreNewt::Body* me = static_cast<OgreNewt::Body*>(NewtonBodyGetUserData(visitor));
178 if (me)
179 {
182 }
183 }
184
185
186 void _CDECL Body::newtonForceTorqueCallback(const NewtonBody* body, dFloat timeStep, int threadIndex)
187 {
188 OgreNewt::Body* me = static_cast<OgreNewt::Body*>(NewtonBodyGetUserData(body));
189 if (me)
190 {
191 if (me->m_forcecallback)
192 me->m_forcecallback(me, Ogre::Real(timeStep), threadIndex);
193 }
194 }
195
196 void Body::standardForceCallback(OgreNewt::Body* me, dFloat timestep, int threadIndex)
197 {
198 if (me)
199 {
200 //apply a simple gravity force.
201 Ogre::Real mass;
202 Ogre::Vector3 inertia;
203
204 me->getMassMatrix(mass, inertia);
205 Ogre::Vector3 gravityAcceleration(0.0f, -9.8f, 0.0f);
206 Ogre::Vector3 gravityForce = gravityAcceleration * mass;
207
208 me->addForce(gravityForce);
209
210 int nth = 0;
211
212 for (std::vector<std::pair<Ogre::Vector3, Ogre::Vector3> >::const_iterator it = me->m_accumulatedGlobalForces.begin(); it != me->m_accumulatedGlobalForces.end(); it++)
213 {
214 Ogre::LogManager::getSingleton().getDefaultLog()->logMessage("# [" + Ogre::StringConverter::toString(++nth) + "] force " + Ogre::StringConverter::toString(it->first) + " at " + Ogre::StringConverter::toString(it->second) + " to " + me->getOgreNode()->getName());
215
216 me->addGlobalForce(it->first, it->second);
217 }
218
219 me->m_accumulatedGlobalForces.clear();
220 }
221 }
222
223 // attachToNode
224 void Body::attachNode(Ogre::Node* node)
225 {
226 m_node = node;
227 node->_update(false, true);
228 updateNode(1.0f);
229 }
230
231 void Body::enableSimulation(bool state)
232 {
233 if (m_body)
234 {
235 if (state)
236 {
237 //NewtonBodyEnableSimulation(m_body);
238 unFreeze();
239 }
240 else
241 {
242 //NewtonBodyDisableSimulation(m_body); // this destroy joints
243 freeze();
244 }
245 }
246 }
247
248 void Body::setPositionOrientation(const Ogre::Vector3& pos, const Ogre::Quaternion& orient, int threadIndex)
249 {
250 if (m_body)
251 {
252 dFloat matrix[16];
253
254 // reset the previews Position and rotation for this body
255 m_curPosit = pos;
256 m_prevPosit = pos;
257 m_curRotation = orient;
258 m_prevRotation = orient;
259
260 OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
261
264 NewtonBodySetMatrix(m_body, &matrix[0]);
266 updateNode(1.0f);
267 }
268 }
269
270 void Body::updatePositionOrientation(const Ogre::Vector3& pos, const Ogre::Quaternion& orient, int threadIndex)
271 {
272 if (m_body)
273 {
274 dFloat matrix[16];
275
276 // reset the previews Position and rotation for this body
277 m_curPosit = pos;
278 m_prevPosit = pos;
279 m_curRotation = orient;
280 m_prevRotation = orient;
281
282 OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
285 NewtonBodySetMatrix(m_body, &matrix[0]);
287 }
288 }
289
290 //$BB add : set body position from node without node update
292 {
293 if (m_body && m_node)
294 {
295 dFloat matrix[16];
296
297 // reset the previews Position and rotation for this body
298 // updated before the function call
299 //m_node->_update(false, true);
300 m_curPosit = m_node->_getDerivedPosition();
301 m_curRotation = m_node->_getDerivedOrientation();
304
308 NewtonBodySetMatrix(m_body, &matrix[0]);
310 }
311 }
312
313 // set mass matrix
314 void Body::setMassMatrix(Ogre::Real mass, const Ogre::Vector3& inertia)
315 {
316 if (m_body)
317 {
319 NewtonBodySetMassMatrix(m_body, (float)mass, (float)inertia.x, (float)inertia.y, (float)inertia.z);
320 }
321 }
322
323 void Body::setMass(Ogre::Real mass)
324 {
325 if (m_body)
326 {
328 NewtonBodySetMassProperties(m_body, (float)mass, NewtonBodyGetCollision(m_body));
329 }
330 }
331
332 // basic gravity callback
334 {
335 setCustomForceAndTorqueCallback(standardForceCallback);
336 }
337
338 // custom user force callback
340 {
341 if (!m_forcecallback)
342 {
343 m_forcecallback = callback;
344 NewtonBodySetForceAndTorqueCallback(m_body, newtonForceTorqueCallback);
345 }
346 else
347 {
348 m_forcecallback = callback;
349 }
350 }
351
356
361
366
367 /*
368 // custom user force callback
369 void Body::setCustomTransformCallback( TransformCallback callback )
370 {
371 if (!m_transformcallback)
372 {
373 m_transformcallback = callback;
374 NewtonBodySetTransformCallback( m_body, newtonTransformCallback );
375 }
376 else
377 {
378 m_transformcallback = callback;
379 }
380 }
381 */
382
383 //set collision
385 {
386 m_collision = col;
388 NewtonBodySetCollision(m_body, col->getNewtonCollision());
389 NewtonBodySetMassProperties(m_body, (float)0.0f, NewtonBodyGetCollision(m_body));
390 }
391
392 //get collision
394 {
395 return m_collision;
396 }
397
398 //get material group ID
400 {
401 if (m_matid)
402 return m_matid;
403 else
405 }
406
407 void Body::setScale(const Ogre::Vector3& scale, const Ogre::Quaternion& orient, Ogre::Vector3& pos)
408 {
409 if (m_scale == scale)
410 return;
411
412 dFloat matrix[16];
413 m_scale = scale;
414 NewtonCollision* collision = NewtonBodyGetCollision(m_body);
415 OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
416
419 NewtonCollisionSetMatrix(collision, matrix);
420 NewtonBodySetCollisionScale(m_body, m_scale.x, m_scale.y, m_scale.z);
422 }
423
424 // get position and orientation
425 void Body::getPositionOrientation(Ogre::Vector3& pos, Ogre::Quaternion& orient) const
426 {
427 // Ogre::Real matrix[16];
428 // NewtonBodyGetMatrix ( m_body, matrix );
429 // NewtonBodyGetRotation ( m_body, &orient.w );
430 // pos = Ogre::Vector3( matrix[12], matrix[13], matrix[14] );
431
432 pos = m_curPosit;
433 orient = m_curRotation;
434 }
435
436 Ogre::Vector3 Body::getPosition() const
437 {
438 return m_curPosit;
439 }
440
441
442 Ogre::Quaternion Body::getOrientation() const
443 {
444 return m_curRotation;
445 }
446
448 void Body::getVisualPositionOrientation(Ogre::Vector3& pos, Ogre::Quaternion& orient) const
449 {
450 pos = m_nodePosit;
451 orient = m_nodeRotation;
452 }
453
454 Ogre::Vector3 Body::getVisualPosition() const
455 {
456 return m_nodePosit;
457 }
458
459
460 Ogre::Quaternion Body::getVisualOrientation() const
461 {
462 return m_nodeRotation;
463 }
464
465 Ogre::AxisAlignedBox Body::getAABB() const
466 {
467 Ogre::AxisAlignedBox ret;
468 dVector min, max;
469
470 NewtonBodyGetAABB(m_body, &min.m_x, &max.m_x);
471
472 Ogre::Vector3 dmin;
473 Ogre::Vector3 dmax;
474
475 dmin.x = Ogre::Real(min.m_x);
476 dmin.y = Ogre::Real(min.m_y);
477 dmin.z = Ogre::Real(min.m_z);
478
479 dmax.x = Ogre::Real(max.m_x);
480 dmax.y = Ogre::Real(max.m_y);
481 dmax.z = Ogre::Real(max.m_z);
482
483 ret.setExtents(dmin, dmax);
484 return ret;
485 }
486
487 void Body::getMassMatrix(Ogre::Real& mass, Ogre::Vector3& inertia) const
488 {
489 dFloat fMass;
490 dVector fInertia;
491 NewtonBodyGetMassMatrix(m_body, &fMass, &fInertia.m_x, &fInertia.m_y, &fInertia.m_z);
492
493 mass = Ogre::Real(fMass);
494 inertia.x = Ogre::Real(fInertia.m_x);
495 inertia.y = Ogre::Real(fInertia.m_y);
496 inertia.z = Ogre::Real(fInertia.m_z);
497 }
498
499 Ogre::Real Body::getMass() const
500 {
501 dFloat fMass;
502 dVector fInertia;
503 NewtonBodyGetMassMatrix(m_body, &fMass, &fInertia.m_x, &fInertia.m_y, &fInertia.m_z);
504
505 return Ogre::Real(fMass);
506 }
507
508 Ogre::Vector3 Body::getInertia() const
509 {
510 dFloat fMass;
511 dVector fInertia;
512 Ogre::Vector3 inertia;
513 NewtonBodyGetMassMatrix(m_body, &fMass, &fInertia.m_x, &fInertia.m_y, &fInertia.m_z);
514
515 inertia.x = Ogre::Real(fInertia.m_x);
516 inertia.y = Ogre::Real(fInertia.m_y);
517 inertia.z = Ogre::Real(fInertia.m_z);
518 return inertia;
519 }
520
521 void Body::getInvMass(Ogre::Real& mass, Ogre::Vector3& inertia) const
522 {
523 dFloat fMass;
524 dVector fInertia;
525 NewtonBodyGetInvMass(m_body, &fMass, &fInertia.m_x, &fInertia.m_y, &fInertia.m_z);
526
527 mass = Ogre::Real(fMass);
528 inertia.x = Ogre::Real(fInertia.m_x);
529 inertia.y = Ogre::Real(fInertia.m_y);
530 inertia.z = Ogre::Real(fInertia.m_z);
531 }
532
533 Ogre::Vector3 Body::getOmega() const
534 {
535 //m_world->waitForUpdateToFinish();
536 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
537 dVector fVec;
538 NewtonBodyGetOmega(m_body, &fVec.m_x);
539 if (!_isnan(ret.x))
540 ret.x = Ogre::Real(fVec.m_x);
541 if (!_isnan(ret.y))
542 ret.y = Ogre::Real(fVec.m_y);
543 if (!_isnan(ret.z))
544 ret.z = Ogre::Real(fVec.m_z);
545 return ret;
546 }
547
548 Ogre::Vector3 Body::getVelocity() const
549 {
550 //m_world->waitForUpdateToFinish();
551 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
552 dVector fVec;
553 NewtonBodyGetVelocity(m_body, &fVec.m_x);
554 if (!_isnan(ret.x))
555 ret.x = Ogre::Real(fVec.m_x);
556 if (!_isnan(ret.y))
557 ret.y = Ogre::Real(fVec.m_y);
558 if (!_isnan(ret.z))
559 ret.z = Ogre::Real(fVec.m_z);
560 return ret;
561 }
562
563 Ogre::Vector3 Body::getForce() const
564 {
565 //m_world->waitForUpdateToFinish();
566 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
567 dVector fVec;
568 NewtonBodyGetForce(m_body, &fVec.m_x);
569 if (!_isnan(ret.x))
570 ret.x = Ogre::Real(fVec.m_x);
571 if (!_isnan(ret.y))
572 ret.y = Ogre::Real(fVec.m_y);
573 if (!_isnan(ret.z))
574 ret.z = Ogre::Real(fVec.m_z);
575 return ret;
576 }
577
578 Ogre::Vector3 Body::getTorque() const
579 {
580 //m_world->waitForUpdateToFinish();
581 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
582 dVector fVec;
583 NewtonBodyGetTorque(m_body, &fVec.m_x);
584 if (!_isnan(ret.x))
585 ret.x = Ogre::Real(fVec.m_x);
586 if (!_isnan(ret.y))
587 ret.y = Ogre::Real(fVec.m_y);
588 if (!_isnan(ret.z))
589 ret.z = Ogre::Real(fVec.m_z);
590 return ret;
591 }
592
593 Ogre::Vector3 Body::getForceAcceleration() const
594 {
595 //m_world->waitForUpdateToFinish();
596 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
597 dVector fVec;
598 NewtonBodyGetForceAcc(m_body, &fVec.m_x);
599 if (!_isnan(ret.x))
600 ret.x = Ogre::Real(fVec.m_x);
601 if (!_isnan(ret.y))
602 ret.y = Ogre::Real(fVec.m_y);
603 if (!_isnan(ret.z))
604 ret.z = Ogre::Real(fVec.m_z);
605 return ret;
606 }
607
608 Ogre::Vector3 Body::getTorqueAcceleration() const
609 {
610 //m_world->waitForUpdateToFinish();
611 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
612 dVector fVec;
613 NewtonBodyGetTorqueAcc(m_body, &fVec.m_x);
614 if (!_isnan(ret.x))
615 ret.x = Ogre::Real(fVec.m_x);
616 if (!_isnan(ret.y))
617 ret.y = Ogre::Real(fVec.m_y);
618 if (!_isnan(ret.z))
619 ret.z = Ogre::Real(fVec.m_z);
620 return ret;
621 }
622
623 Ogre::Vector3 Body::calculateInverseDynamicsForce(Ogre::Real timestep, Ogre::Vector3 desiredVelocity)
624 {
625 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
626 dVector fVec;
627 dVector fVel;
628 fVel.m_x = dFloat(desiredVelocity.x);
629 fVel.m_y = dFloat(desiredVelocity.y);
630 fVel.m_z = dFloat(desiredVelocity.z);
631
632 NewtonBodyCalculateInverseDynamicsForce(m_body, dFloat(timestep), &fVel.m_x, &fVec.m_x);
633 if (!_isnan(ret.x))
634 ret.x = Ogre::Real(fVec.m_x);
635 if (!_isnan(ret.y))
636 ret.y = Ogre::Real(fVec.m_y);
637 if (!_isnan(ret.z))
638 ret.z = Ogre::Real(fVec.m_z);
639 return ret;
640 }
641
642
643 Ogre::Vector3 Body::getAngularDamping() const
644 {
645 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
646 dVector fVec;
647 NewtonBodyGetAngularDamping(m_body, &fVec.m_x);
648 if (!_isnan(ret.x))
649 ret.x = Ogre::Real(fVec.m_x);
650 if (!_isnan(ret.y))
651 ret.y = Ogre::Real(fVec.m_y);
652 if (!_isnan(ret.z))
653 ret.z = Ogre::Real(fVec.m_z);
654 return ret;
655 }
656
657 Ogre::Vector3 Body::getCenterOfMass() const
658 {
659 Ogre::Vector3 ret = Ogre::Vector3::ZERO;
660 dVector fVec;
661 NewtonBodyGetCentreOfMass(m_body, &fVec.m_x);
662 if (!_isnan(ret.x))
663 ret.x = Ogre::Real(fVec.m_x);
664 if (!_isnan(ret.y))
665 ret.y = Ogre::Real(fVec.m_y);
666 if (!_isnan(ret.z))
667 ret.z = Ogre::Real(fVec.m_z);
668 return ret;
669 }
670
671 Ogre::Vector3 Body::calculateOffset() const
672 {
673 Ogre::Vector3 offset = Ogre::Vector3::ZERO;
674 dVector fIner;
675 dVector fOff;
676
677 if (m_body)
678 {
679 NewtonConvexCollisionCalculateInertialMatrix(NewtonBodyGetCollision(m_body), &fIner.m_x, &fOff.m_x);
680
681 offset.x = Ogre::Real(fOff.m_x);
682 offset.y = Ogre::Real(fOff.m_y);
683 offset.z = Ogre::Real(fOff.m_z);
684 }
685
686 return offset;
687 }
688
689 Ogre::Vector3 Body::calculateInertialMatrix() const
690 {
691 Ogre::Vector3 inertia = Ogre::Vector3::ZERO;
692 dVector fIner;
693 dVector fOff;
694
695 if (m_body)
696 {
697 NewtonConvexCollisionCalculateInertialMatrix(NewtonBodyGetCollision(m_body), &fIner.m_x, &fOff.m_x);
698
699 inertia.x = Ogre::Real(fIner.m_x);
700 inertia.y = Ogre::Real(fIner.m_y);
701 inertia.z = Ogre::Real(fIner.m_z);
702 }
703
704 return inertia;
705 }
706
707 void Body::addGlobalForce(const Ogre::Vector3& force, const Ogre::Vector3& pos)
708 {
709 Ogre::Vector3 bodypos;
710 Ogre::Quaternion bodyorient;
711 getPositionOrientation(bodypos, bodyorient);
712
713 Ogre::Vector3 localMassCenter = getCenterOfMass();
714 Ogre::Vector3 globalMassCenter = bodypos + bodyorient * localMassCenter;
715
716 Ogre::Vector3 topoint = pos - globalMassCenter;
717 Ogre::Vector3 torque = topoint.crossProduct(force);
718
719 addForce(force);
720 addTorque(torque);
721 }
722
723 void Body::addGlobalForceAccumulate(const Ogre::Vector3& force, const Ogre::Vector3& pos)
724 {
725 m_accumulatedGlobalForces.push_back(std::pair<Ogre::Vector3, Ogre::Vector3>(force, pos));
726 }
727
728 void Body::addLocalForce(const Ogre::Vector3& force, const Ogre::Vector3& pos)
729 {
730 Ogre::Vector3 bodypos;
731 Ogre::Quaternion bodyorient;
732
733 getPositionOrientation(bodypos, bodyorient);
734
735 Ogre::Vector3 globalforce = bodyorient * force;
736 Ogre::Vector3 globalpoint = (bodyorient * pos) + bodypos;
737
738 addGlobalForce(globalforce, globalpoint);
739 }
740
742 {
743 NewtonBody* body = NewtonWorldGetNextBody(m_world->getNewtonWorld(), m_body);
744 if (body)
745 return (Body*)NewtonBodyGetUserData(body);
746
747 return NULL;
748 }
749
754
759
760 void Body::setVisualPosition(Ogre::Vector3 pos, Ogre::Quaternion quat)
761 {
762 m_nodePosit = pos;
763 m_nodeRotation = quat;
764 }
765
766 void Body::updateNode(Ogre::Real interpolatParam)
767 {
768 Ogre::Vector3 velocity = m_curPosit - m_prevPosit;
769
770 m_nodePosit = m_prevPosit + velocity * interpolatParam;
771 m_nodeRotation = Ogre::Quaternion::Slerp(interpolatParam, m_prevRotation, m_curRotation);
772
773 if (m_node)
774 {
775 //DO all in SBody callback!
776
777 // Set position in global terms.
778 /*Ogre::Node* m_nodeParent = m_node->getParent();
779 if (m_nodeParent)
780 {
781 //$BB add : force update to be sure of the derivated values
782 m_nodeParent->_update(false, true);
783
784 Ogre::Vector3 gscale = m_nodeParent->_getDerivedScale();
785 //if this is a bad scale we ignore the set position
786 if (gscale.x == 0.0f || gscale.y == 0.0f || gscale.z == 0.0f)
787 return;
788
789 Ogre::Vector3 invgscale = Ogre::Vector3::UNIT_SCALE / gscale;
790 m_node->setPosition((m_nodeParent->_getDerivedOrientation().Inverse() * (m_nodePosit - m_nodeParent->_getDerivedPosition()) * invgscale));
791 m_node->setOrientation((m_nodeParent->_getDerivedOrientation().Inverse() * m_nodeRotation));
792 }
793 else
794 {
795 m_node->setPosition(m_nodePosit);
796 m_node->setOrientation(m_nodeRotation);
797 }*/
798
801
804 }
805 }
806
807 void Body::setVelocity(const Ogre::Vector3& vel)
808 {
809 //m_world->waitForUpdateToFinish();
810 //NewtonWorldConvexCast(m_world,
811 dVector fVel;
812 fVel.m_x = dFloat(vel.x);
813 fVel.m_y = dFloat(vel.y);
814 fVel.m_z = dFloat(vel.z);
815
816 NewtonBodySetVelocity(m_body, &fVel.m_x);
817 }
818
819 // works only in physic listener
820 void Body::MoveToPosition(const Ogre::Vector3 &destPos, const Ogre::Real &stiffness, Ogre::Real timestep)
821 {
822 /*
823 fun getForce(hvec, gvec, vmass, vmassmat, vnbsec, nbframes)=
824 let (divideVectorF (subVectorF hvec gvec) vnbsec) -> vel in
825 let multiplyVectorF (divideVectorF vel vnbsec) vmass -> hforce in
826 [hforce vel];;
827 */
828
829 dMatrix matrix;
830 dVector veloc0;
831 dVector targetPositionInGlobalSpace(destPos.x, destPos.y, destPos.z, 0.0f);
832
833 dFloat invTimeStep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
834 NewtonWorld* const world = NewtonBodyGetWorld(m_body);
835 if (!world)
836 return;
837 NewtonWorldCriticalSectionLock(world, 0);
838
839 // calculate the desired impulse
840 NewtonBodyGetMatrix(m_body, &matrix[0][0]);
841 NewtonBodyGetVelocity(m_body, &veloc0[0]);
842
843 dVector deltaVeloc(targetPositionInGlobalSpace - matrix.m_posit);
844 deltaVeloc = deltaVeloc.Scale(stiffness * invTimeStep);
845
846 // convert the delta velocity change to a external force and torque
847 dFloat Ixx;
848 dFloat Iyy;
849 dFloat Izz;
850 dFloat mass;
851 NewtonBodyGetMassMatrix(m_body, &mass, &Ixx, &Iyy, &Izz);
852 dVector force((deltaVeloc - veloc0).Scale(mass * invTimeStep));
853
854 NewtonBodyAddForce(m_body, &force[0]);
855 NewtonBodySetSleepState(m_body, 0);
856
857 NewtonWorldCriticalSectionUnlock(world);
858 }
859
860 void Body::RotateToOrientation(const Ogre::Quaternion &destquat, const Ogre::Real &stiffness, Ogre::Real timestep)
861 {
862 dMatrix matrix;
863 dVector omega0;
864 dVector fRot;
865
866
867 dFloat invTimeStep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
868 NewtonWorld* const world = NewtonBodyGetWorld(m_body);
869 if (!world)
870 return;
871
872 NewtonWorldCriticalSectionLock(world, 0);
873
874 // calculate the desired impulse
875 NewtonBodyGetMatrix(m_body, &matrix[0][0]);
876 NewtonBodyGetOmega(m_body, &omega0[0]);
877 NewtonBodyGetRotation(m_body, &fRot.m_x);
878
879 // convert the delta velocity change to a external force and torque
880 dFloat Ixx;
881 dFloat Iyy;
882 dFloat Izz;
883 dFloat mass;
884 NewtonBodyGetMassMatrix(m_body, &mass, &Ixx, &Iyy, &Izz);
885 dVector angularMomentum(Ixx, Iyy, Izz);
886
887 Ogre::Quaternion rot(Ogre::Real(fRot.m_x), Ogre::Real(fRot.m_y), Ogre::Real(fRot.m_z), Ogre::Real(fRot.m_w));
888 Ogre::Quaternion diffq = destquat * rot.Inverse();
889 Ogre::Euler euler(diffq);
890 dVector domega(euler.getPitch().valueRadians(), euler.getYaw().valueRadians(), euler.getRoll().valueRadians());
891 domega = domega.Scale(invTimeStep);
892 domega = domega - omega0;
893 //multiplyVectorF (divideVectorF omega vnbsec) vmassmat
894
895 angularMomentum = matrix.RotateVector(angularMomentum.CompProduct(matrix.UnrotateVector(domega)));
896 dVector torque(angularMomentum.Scale(invTimeStep * stiffness));
897
898 NewtonBodyAddTorque(m_body, &torque[0]);
899
900 NewtonBodySetSleepState(m_body, 0);
901 NewtonWorldCriticalSectionUnlock(world);
902 }
903
904 // --------------------------------------------------------------------------------------
905
906
907}
Class for Euler rotations.
Definition SO3Euler.h:25
Ogre::Radian getYaw() const
Get the Yaw angle.
Definition SO3Euler.h:70
Ogre::Radian getRoll() const
Get the Roll angle.
Definition SO3Euler.h:76
Ogre::Radian getPitch() const
Get the Pitch angle.
Definition SO3Euler.h:73
main class for all Rigid Bodies in the system.
void MoveToPosition(const Ogre::Vector3 &destPos, const Ogre::Real &stiffness, Ogre::Real timestep)
void updatePositionOrientation(const Ogre::Vector3 &pos, const Ogre::Quaternion &orient, int threadIndex=-1)
void getMassMatrix(Ogre::Real &mass, Ogre::Vector3 &inertia) const
get Ogre::Real(mass) and Ogre::Vector3(inertia) of the body.
Ogre::Real getMass() const
return body mass
void enableSimulation(bool state)
const OgreNewt::CollisionPtr & getCollision() const
set the factors that cause a body to "freeze" when equilibrium reached.
Ogre::Vector3 m_nodePosit
void setScale(const Ogre::Vector3 &scale, const Ogre::Quaternion &orient, Ogre::Vector3 &pos)
set the collision scale
void setCustomForceAndTorqueCallback(ForceCallback callback)
set a custom force callback for this body to use.
Ogre::Quaternion getOrientation() const
returns body orientation
Ogre::Any m_userdata
void setTriggerExitCallback(TriggerExitCallback callback)
void addGlobalForceAccumulate(const Ogre::Vector3 &force, const Ogre::Vector3 &pos)
helper function that adds a force (and resulting torque) to an object in global cordinates called fro...
Ogre::Vector3 getForce() const
get the force acting on the body.
void getInvMass(Ogre::Real &mass, Ogre::Vector3 &inertia) const
get invert mass + inertia for the body.
void setTriggerEnterCallback(TriggerEnterCallback callback)
void addLocalForce(const Ogre::Vector3 &force, const Ogre::Vector3 &pos)
Ogre::Quaternion getVisualOrientation() const
returns body visual orientation
const OgreNewt::MaterialID * getMaterialGroupID() const
get a pointer to the Material assigned to this body.
void setNodeUpdateJointNotify(NodeUpdateNotifyCallback callback)
TriggerInsideCallback m_triggerInsideCallback
void setVisualPosition(Ogre::Vector3 pos, Ogre::Quaternion quat)
void freeze()
freeze the rigid body.
void addGlobalForce(const Ogre::Vector3 &force, const Ogre::Vector3 &pos)
helper function that adds a force (and resulting torque) to an object in global cordinates.
Ogre::Vector3 getVelocity() const
get velocity of the body. in global coordinates.
Ogre::Vector3 getPosition() const
returns body position
std::function< void(OgreNewt::Body *)> TriggerEnterCallback
void setStandardForceCallback()
set a standard gravity callback for this body to use.
ForceCallback m_forcecallback
TriggerEnterCallback m_triggerEnterCallback
Ogre::Vector3 m_scale
Ogre::Vector3 m_prevPosit
void updateNode(Ogre::Real interpolatParam)
update the position of the node (if attached) and sets m_nodeupdateneeded to false
void addTorque(const Ogre::Vector3 &torque)
add torque to the body.
std::function< void(OgreNewt::Body *)> TriggerExitCallback
void setMassMatrix(Ogre::Real mass, const Ogre::Vector3 &inertia)
set the mass and inertia for the body.
void addForce(const Ogre::Vector3 &force)
add force to the body.
Body * getNext() const
use this function to iterate through all bodies
NewtonBody * m_body
NodeUpdateNotifyCallback m_nodeupdatejointnotifycallback
Ogre::Node * m_node
Ogre::Vector3 getVisualPosition() const
return body visual position
Ogre::Vector3 getCenterOfMass() const
get the center of mass.
Ogre::Vector3 getOmega() const
get omega of the body. in global space.
std::function< void(OgreNewt::Body *)> NodeUpdateNotifyCallback
node update notify.
Ogre::Quaternion m_curRotation
std::function< void(OgreNewt::Body *, Ogre::Real timeStep, int threadIndex)> ForceCallback
custom force callback.
std::vector< std::pair< Ogre::Vector3, Ogre::Vector3 > > m_accumulatedGlobalForces
void setPositionOrientationFromNode(int threadIndex=-1)
position and orient the body from node.
void setMass(Ogre::Real mass)
Ogre::Vector3 calculateInverseDynamicsForce(Ogre::Real timestep, Ogre::Vector3 desiredVelocity)
calculate force needed for given velocity
Ogre::Vector3 calculateOffset() const
World *const m_world
Body(World *const W, const OgreNewt::CollisionPtr &col, int bodytype=0, bool trigger=false)
constructor.
void setPositionOrientation(const Ogre::Vector3 &pos, const Ogre::Quaternion &orient, int threadIndex=-1)
position and orient the body arbitrarily.
Ogre::Vector3 getInertia() const
return body inertia
void attachNode(Ogre::Node *node)
attach this body to an Ogre::Node*
~Body()
destructor
const MaterialID * m_matid
TriggerExitCallback m_triggerExitCallback
Ogre::Vector3 getForceAcceleration() const
get the linear acceleration due to forces acting on the body.
NodeUpdateNotifyCallback m_nodeupdatenotifycallback
Ogre::Vector3 m_curPosit
void setNodeUpdateNotify(NodeUpdateNotifyCallback callback)
set a custom node update notify.
Ogre::AxisAlignedBox getAABB() const
get the axis-aligned bounding box for this body.
Ogre::Node * getOgreNode() const
get a pointer to the attached Node.
void setVelocity(const Ogre::Vector3 &vel)
set an arbitrary velocity for the body.
Ogre::Vector3 calculateInertialMatrix() const
Ogre::Vector3 getTorqueAcceleration() const
get the rotational acceleration due to torque acting on the body.
void RotateToOrientation(const Ogre::Quaternion &destquat, const Ogre::Real &stiffness, Ogre::Real timestep)
void getVisualPositionOrientation(Ogre::Vector3 &pos, Ogre::Quaternion &orient) const
get the node position and orientation in form of an Ogre::Vector(position) and Ogre::Quaternion(orien...
Ogre::Vector3 getAngularDamping() const
get angular damping
OgreNewt::CollisionPtr m_collision
Ogre::Vector3 getTorque() const
get the torque acting on the body.
void setCollision(const OgreNewt::CollisionPtr &col)
set the collision that represents the shape of the body
Ogre::Quaternion m_prevRotation
std::function< void(OgreNewt::Body *)> TriggerInsideCallback
void setTriggerInsideCallback(TriggerInsideCallback callback)
Ogre::Quaternion m_nodeRotation
void getPositionOrientation(Ogre::Vector3 &pos, Ogre::Quaternion &orient) const
get position and orientation in form of an Ogre::Vector(position) and Ogre::Quaternion(orientation)
void unFreeze()
unfreeze the rigid body.
represents a shape for collision detection
NewtonCollision *const getNewtonCollision() const
retrieve the Newton pointer
represents a material
represents a physics world.
void DestroyController(CustomTriggerController *const controller)
void criticalSectionUnlock() const
notify the exit of a critical section of code.
const MaterialID * getDefaultMaterialID() const
get the default materialID object.
NewtonWorld * getNewtonWorld() const
retrieves a pointer to the NewtonWorld
void criticalSectionLock() const
notify an entrance to a critical section of code.
CustomTriggerController * CreateController(const dMatrix &matrix, NewtonCollision *const convexShape)
void waitForUpdateToFinish() const
void setTriggerInsideCallback(WorldTriggerInsideCallback callback)
void setTriggerExitCallback(WorldTriggerExitCallback callback)
void setTriggerEnterCallback(WorldTriggerEnterCallback callback)
_OgreNewtExport void QuatPosToMatrix(const Ogre::Quaternion &quat, const Ogre::Vector3 &pos, dFloat *matrix)
Take a Quaternion and Position Matrix and create a Newton-happy float matrix!