source: trunk/test/BtPhysTest.cpp @ 1722

Last change on this file since 1722 was 1722, checked in by touky, 9 years ago

LocalInertia error FIX.
Character integration better but still not working.

File size: 16.7 KB
Line 
1//
2// BtPhysTest
3//
4// Copyright: (c) 2009-2012 Benjamin Huet <huet.benjamin@gmail.com>
5//            (c) 2012 Sam Hocevar <sam@hocevar.net>
6//
7
8#if defined HAVE_CONFIG_H
9#   include "config.h"
10#endif
11
12#if defined _WIN32
13#   include <direct.h>
14#endif
15
16#if defined _XBOX
17#   define _USE_MATH_DEFINES /* for M_PI */
18#   include <xtl.h>
19#   undef near /* Fuck Microsoft */
20#   undef far /* Fuck Microsoft again */
21#elif defined _WIN32
22#   define _USE_MATH_DEFINES /* for M_PI */
23#   define WIN32_LEAN_AND_MEAN
24#   include <windows.h>
25#   undef near /* Fuck Microsoft */
26#   undef far /* Fuck Microsoft again */
27#else
28#   include <cmath>
29#endif
30
31#include "core.h"
32#include "loldebug.h"
33
34using namespace lol;
35
36#ifndef HAVE_PHYS_USE_BULLET
37#define HAVE_PHYS_USE_BULLET
38#endif /* HAVE_PHYS_USE_BULLET */
39
40#include "Physics/LolPhysics.h"
41#include "Physics/EasyPhysics.h"
42#include "PhysicObject.h"
43#include "BtPhysTest.h"
44
45using namespace lol::phys;
46
47#define CUBE_HALF_EXTENTS .5f
48#define EXTRA_HEIGHT 1.f
49
50int gNumObjects = 64;
51
52#define USE_WALL                1
53#define USE_PLATFORM    1
54#define USE_ROPE                0
55#define USE_BODIES              0
56#define USE_ROTATION    0
57#define USE_CHARACTER   1
58
59BtPhysTest::BtPhysTest(bool editor)
60{
61    /* Create a camera that matches the settings of XNA BtPhysTest */
62    m_camera = new Camera(vec3(0.f, 600.f, 0.f),
63                          vec3(0.f, 0.f, 0.f),
64                          vec3(0, 1, 0));
65    m_camera->SetRotation(quat::fromeuler_xyz(0.f, 0.f, 0.f));
66    m_camera->SetPerspective(45.f, 1280.f, 960.f, .1f, 1000.f);
67        //m_camera->SetOrtho(1280.f / 6, 960.f / 6, -1000.f, 1000.f);
68    Ticker::Ref(m_camera);
69
70    m_ready = false;
71
72        m_simulation = new Simulation();
73        m_simulation->Init();
74        vec3 NewGravity = vec3(.0f, -10.0f, .0f);
75        m_simulation->SetGravity(NewGravity);
76        m_simulation->SetContinuousDetection(true);
77        m_simulation->SetTimestep(1.f / 120.f);
78    Ticker::Ref(m_simulation);
79
80        float offset = 29.5f;
81        vec3 pos_offset = vec3(.0f, 30.f, .0f);
82        if (USE_WALL)
83        {
84                for (int i=0; i < 6; i++)
85                {
86                        vec3 NewPosition = vec3(.0f);
87                        quat NewRotation = quat(1.f);
88
89                        PhysicsObject* NewPhyobj = new PhysicsObject(m_simulation, NewPosition, NewRotation);
90
91                        int idx = i/2;
92                        NewPosition = pos_offset;
93                        NewPosition[idx] += offset;
94                        offset *= -1.f;
95
96                        if (idx != 1)
97                        {
98                                vec3 axis = vec3(.0f);
99                                axis[2 - idx] = 1;
100                                NewRotation = quat::rotate(90.f, axis);
101                        }
102
103                        NewPhyobj->SetTransform(NewPosition, NewRotation);
104                        Ticker::Ref(NewPhyobj);
105                        m_ground_list << NewPhyobj;
106                }
107        }
108
109        if (USE_PLATFORM)
110        {
111                quat NewRotation = quat::fromeuler_xyz(5.f, 0.f, 0.f);
112                vec3 NewPosition = pos_offset + vec3(5.0f, -20.0f, -15.0f);
113
114                PhysicsObject* NewPhyobj = new PhysicsObject(m_simulation, NewPosition, NewRotation, 1);
115
116                m_platform_list << NewPhyobj;
117                Ticker::Ref(NewPhyobj);
118
119                NewPosition = pos_offset + vec3(-20.0f, -25.0f, 5.0f);
120
121                NewPhyobj = new PhysicsObject(m_simulation, NewPosition, NewRotation, 1);
122
123                m_platform_list << NewPhyobj;
124                Ticker::Ref(NewPhyobj);
125        }
126
127        if (USE_CHARACTER)
128        {
129                quat NewRotation = quat::fromeuler_xyz(0.f, 0.f, 0.f);
130                vec3 NewPosition = pos_offset + vec3(.0f, 40.0f, .0f);
131
132                PhysicsObject* NewPhyobj = new PhysicsObject(m_simulation, NewPosition, NewRotation, 2);
133
134                m_character_list << NewPhyobj;
135                Ticker::Ref(NewPhyobj);
136        }
137
138        if (USE_BODIES)
139        {
140                for (int x=0; x < 6; x++)
141                {
142                        for (int y=0; y < 6; y++)
143                        {
144                                for (int z=0; z < 5; z++)
145                                {
146                                        PhysicsObject* new_physobj = new PhysicsObject(m_simulation, 1000.f,
147                                                vec3(-20.f, 15.f, -20.f) +
148                                                vec3(8.f * (float)x, 8.f * (float)y, 8.f * (float)z));
149                                        m_physobj_list << new_physobj;
150                                        Ticker::Ref(new_physobj);
151                                }
152                        }
153                }
154        }
155
156        if (USE_ROPE)
157        {
158                Array<PhysicsObject*> RopeElements;
159                for (int i = 0; i < 14; i++)
160                {
161                        PhysicsObject* new_physobj = new PhysicsObject(m_simulation, 1000.f,
162                                vec3(0.f, 15.f, -20.f) +
163                                vec3(0.f, 0.f, 2.f * (float)i), 1);
164                        RopeElements << new_physobj;
165                        m_physobj_list << new_physobj;
166                        Ticker::Ref(new_physobj);
167                        if (RopeElements.Count() > 1)
168                        {
169                                EasyConstraint* new_constraint = new EasyConstraint();
170
171                                vec3 A2B = .5f * (RopeElements[i]->GetPhysic()->GetTransform().v3.xyz -
172                                                        RopeElements[i - 1]->GetPhysic()->GetTransform().v3.xyz);
173                                new_constraint->SetPhysObjA(RopeElements[i - 1]->GetPhysic(), lol::mat4::translate(A2B));
174                                new_constraint->SetPhysObjB(RopeElements[i]->GetPhysic(), lol::mat4::translate(-A2B));
175                                new_constraint->InitConstraintToPoint2Point();
176                                new_constraint->DisableCollisionBetweenObjs(true);
177                                new_constraint->AddToSimulation(m_simulation);
178                                m_constraint_list << new_constraint;
179                        }
180                }
181        }
182
183#if 0
184        //init Physics
185        {
186                m_bt_ccd_mode = USE_CCD;
187
188                //collision configuration contains default setup for memory, collision setup
189                m_bt_collision_config = new btDefaultCollisionConfiguration();
190
191                //use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
192                m_bt_dispatcher = new btCollisionDispatcher(m_bt_collision_config);
193                m_bt_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,
194                                                                                                        BOX_SHAPE_PROXYTYPE,
195                                                                                                        m_bt_collision_config->getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE,
196                                                                                                                                                                                                                        CONVEX_SHAPE_PROXYTYPE));
197
198                m_bt_broadphase = new btDbvtBroadphase();
199
200                ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
201                m_bt_solver = new btSequentialImpulseConstraintSolver;
202
203                m_bt_world = new btDiscreteDynamicsWorld(m_bt_dispatcher, m_bt_broadphase, m_bt_solver, m_bt_collision_config);
204                //m_bt_world->setDebugDrawer(&sDebugDrawer);
205                m_bt_world->getSolverInfo().m_splitImpulse = true;
206                m_bt_world->getSolverInfo().m_numIterations = 20;
207
208                m_bt_world->getDispatchInfo().m_useContinuous = (m_bt_ccd_mode == USE_CCD);
209                m_bt_world->setGravity(btVector3(0,-10,0));
210
211                ///create a few basic rigid bodies
212                btBoxShape* box = new btBoxShape(btVector3(btScalar(110.),btScalar(1.),btScalar(110.)));
213                btCollisionShape* groundShape = box;
214                m_bt_collision_shapes << groundShape;
215        m_ground_mesh.Compile("[sc#ddd afcb220 2 220 -1]");
216
217                //m_bt_collision_shapes << new btCylinderShape(btVector3(.5f,.5f,.5f));
218
219                btTransform groundTransform;
220                groundTransform.setIdentity();
221
222                //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
223                {
224                        btScalar mass(0.);
225
226                        //rigidbody is dynamic if and only if mass is non zero, otherwise static
227                        bool isDynamic = (mass != 0.f);
228
229                        btVector3 localInertia(0,0,0);
230                        if (isDynamic)
231                                groundShape->calculateLocalInertia(mass,localInertia);
232
233                        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
234                        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
235                        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
236                        btRigidBody* body = new btRigidBody(rbInfo);
237
238                        //add the body to the dynamics world
239                        m_bt_world->addRigidBody(body);
240                }
241
242                //Adding Shapes
243                {
244                        //create a few dynamic rigidbodies
245                        // Re-using the same collision is better for memory usage and performance
246                        btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
247                        m_rigid_mesh[0].Compile("[sc#add afcb2 2 2 -.1]");
248                        m_rigid_mesh[1].Compile("[sc#dad afcb2 2 2 -.1]");
249                        m_rigid_mesh[2].Compile("[sc#dda afcb2 2 2 -.1]");
250                        m_rigid_mesh[3].Compile("[sc#daa afcb2 2 2 -.1]");
251                        m_rigid_mesh[4].Compile("[sc#ada afcb2 2 2 -.1]");
252                        m_rigid_mesh[5].Compile("[sc#aad afcb2 2 2 -.1]");
253
254                        m_bt_collision_shapes << colShape;
255                        m_bt_dynamic_shapes << colShape;
256
257                        /// Create Dynamic Objects
258                        btTransform startTransform;
259                        startTransform.setIdentity();
260                        btScalar mass(1.f);
261
262                        //rigidbody is dynamic if and only if mass is non zero, otherwise static
263                        bool isDynamic = (mass != 0.f);
264
265                        btVector3 localInertia(0,0,0);
266                        if (isDynamic)
267                                colShape->calculateLocalInertia(mass,localInertia);
268
269                        int i;
270                        for (i=0;i<gNumObjects;i++)
271                        {
272                                btCollisionShape* shape = colShape;
273                                btTransform trans;
274                                trans.setIdentity();
275
276                                //stack them
277                                int colsize = 10;
278                                int row = int(((float)i*CUBE_HALF_EXTENTS*2.0f)/((float)colsize*2.0f*CUBE_HALF_EXTENTS));
279                                int row2 = row;
280                                int col = (i)%(colsize)-colsize/2;
281
282                                if (col>3)
283                                {
284                                        col=11;
285                                        row2 |=1;
286                                }
287
288                                btVector3 pos(((row+col+row2) % 4)*CUBE_HALF_EXTENTS,
289                                              20.0f + row*8*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,
290                                              col*8*CUBE_HALF_EXTENTS + 2 * (row2%2)*CUBE_HALF_EXTENTS);
291
292                                trans.setOrigin(pos);
293       
294                                float mass = 1.f;
295
296
297                                btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
298
299                                //rigidbody is dynamic if and only if mass is non zero, otherwise static
300                                bool isDynamic = (mass != 0.f);
301
302                                btVector3 localInertia(0,0,0);
303                                if (isDynamic)
304                                        shape->calculateLocalInertia(mass,localInertia);
305
306                                //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
307
308                                btDefaultMotionState* myMotionState = new btDefaultMotionState(trans);
309
310                                btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia);
311
312                                btRigidBody* body = new btRigidBody(cInfo);
313                                body->setContactProcessingThreshold(BT_LARGE_FLOAT);
314
315                                m_bt_world->addRigidBody(body);
316
317                                ///when using m_ccdMode
318                                if (m_bt_ccd_mode == USE_CCD)
319                                {
320                                        body->setCcdMotionThreshold(CUBE_HALF_EXTENTS);
321                                        body->setCcdSweptSphereRadius(0.9*CUBE_HALF_EXTENTS);
322                                }
323                        }
324                }
325        }
326#endif
327}
328
329void BtPhysTest::TickGame(float seconds)
330{
331    WorldEntity::TickGame(seconds);
332
333    if (Input::GetButtonState(27 /*SDLK_ESCAPE*/))
334        Ticker::Shutdown();
335
336        vec3 GroundBarycenter = vec3(.0f);
337        vec3 PhysObjBarycenter = vec3(.0f);
338        float factor = .0f;
339
340        if (USE_WALL)
341        {
342                for (int i = 0; i < m_ground_list.Count(); i++)
343                {
344                        PhysicsObject* PhysObj = m_ground_list[i];
345                        mat4 GroundMat = PhysObj->GetTransform();
346
347                        GroundBarycenter += GroundMat.v3.xyz;
348                        factor += 1.f;
349                }
350
351                GroundBarycenter /= factor;
352
353                for (int i = 0; i < m_ground_list.Count(); i++)
354                {
355                        PhysicsObject* PhysObj = m_ground_list[i];
356
357                        mat4 GroundMat = PhysObj->GetTransform();
358                        vec3 CenterToGround = GroundMat.v3.xyz - GroundBarycenter;
359                        vec3 CenterToCam = m_camera->m_position - GroundBarycenter;
360
361                        if (dot(normalize(CenterToCam - CenterToGround),
362                                        normalize(CenterToGround)) > 0.f)
363                                PhysObj->SetRender(false);
364                        else
365                                PhysObj->SetRender(true);
366                }
367        }
368
369        if (USE_ROTATION)
370        {
371                for (int i = 0; i < m_ground_list.Count(); i++)
372                {
373                        PhysicsObject* PhysObj = m_ground_list[i];
374
375                        mat4 GroundMat = PhysObj->GetTransform();
376                        mat4 CenterMx = mat4::translate(GroundBarycenter);
377                        GroundMat = inverse(CenterMx) * GroundMat;
378                        GroundMat = CenterMx *
379                                                mat4(quat::fromeuler_xyz(vec3(.0f, 20.f, 20.0f) * seconds))
380                                                * GroundMat;
381                        PhysObj->SetTransform(GroundMat.v3.xyz, quat(GroundMat));
382                }
383        }
384
385        if (USE_PLATFORM)
386        {
387                for (int i = 0; i < m_platform_list.Count(); i++)
388                {
389                        PhysicsObject* PhysObj = m_platform_list[i];
390
391                        mat4 GroundMat = PhysObj->GetTransform();
392                        if (i == 0)
393                        {
394                                GroundMat = GroundMat * mat4(quat::fromeuler_xyz(vec3(20.f, .0f, .0f) * seconds));
395                                PhysObj->SetTransform(GroundMat.v3.xyz, quat(GroundMat));
396                        }
397                        else
398                        {
399                                GroundMat = GroundMat * mat4::translate(vec3(.0f, .0f, 10.0f) * seconds);
400                                if (GroundMat.v3.z > 40.0f)
401                                        GroundMat = GroundMat * mat4::translate(vec3(.0f, .0f, -80.0f));
402                                PhysObj->SetTransform(GroundMat.v3.xyz, quat(GroundMat));
403                        }
404                }
405        }
406
407        if (USE_CHARACTER)
408        {
409                for (int i = 0; i < m_character_list.Count(); i++)
410                {
411                        PhysicsObject* PhysObj = m_character_list[i];
412                        mat4 GroundMat = PhysObj->GetTransform();
413
414                        PhysObjBarycenter += GroundMat.v3.xyz;
415                        factor += 1.f;
416                }
417
418                PhysObjBarycenter /= factor;
419
420                m_camera->SetTarget(PhysObjBarycenter);
421                m_camera->SetPosition(PhysObjBarycenter + vec3(-80.0f, 80.0f, .0f));
422        }
423        else
424        {
425                PhysObjBarycenter = vec3(.0f);
426                for (int i = 0; i < m_physobj_list.Count(); i++)
427                {
428                        PhysicsObject* PhysObj = m_physobj_list[i];
429                        mat4 GroundMat = PhysObj->GetTransform();
430
431                        PhysObjBarycenter += GroundMat.v3.xyz;
432                        factor += 1.f;
433                }
434
435                PhysObjBarycenter /= factor;
436
437                m_camera->SetTarget(PhysObjBarycenter);
438                m_camera->SetPosition(GroundBarycenter + normalize(GroundBarycenter - PhysObjBarycenter) * 60.0f);
439        }
440
441#if 0
442        ///step the simulation
443        if (m_bt_world)
444        {
445                //int steps = (int)(seconds / 0.005f);
446                //for (int i = 0; i < steps; i++)
447                        m_bt_world->stepSimulation(seconds /*/ steps*/);
448                //optional but useful: debug drawing
449                //m_bt_world->debugDrawWorld();
450        }
451#endif
452}
453
454void BtPhysTest::TickDraw(float seconds)
455{
456    WorldEntity::TickDraw(seconds);
457
458    if (!m_ready)
459    {
460#if 0
461                m_ground_mesh.MeshConvert();
462                m_rigid_mesh[0].MeshConvert();
463                m_rigid_mesh[1].MeshConvert();
464                m_rigid_mesh[2].MeshConvert();
465                m_rigid_mesh[3].MeshConvert();
466                m_rigid_mesh[4].MeshConvert();
467                m_rigid_mesh[5].MeshConvert();
468#endif
469        /* FIXME: this object never cleans up */
470        m_ready = true;
471    }
472
473    //Video::SetClearColor(vec4(0.0f, 0.0f, 0.12f, 1.0f));
474
475#if 0
476        vec3 BarycenterLocation = vec3(.0f);
477        float BarycenterFactor = 0.0f;
478        for(int i=0;i<gNumObjects;i++)
479        {
480                mat4 m(1.0f);
481                btMatrix3x3     rot; rot.setIdentity();
482                btCollisionObject*      colObj = m_bt_world->getCollisionObjectArray()[i];
483                btRigidBody*            body = btRigidBody::upcast(colObj);
484                if(body && body->getMotionState())
485                {
486                        btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
487                        myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(&m[0][0]);
488                        rot = myMotionState->m_graphicsWorldTrans.getBasis();
489                }
490                else
491                {
492                        colObj->getWorldTransform().getOpenGLMatrix(&m[0][0]);
493                        rot = colObj->getWorldTransform().getBasis();
494                }
495                if (i > 0)
496                {
497                        BarycenterLocation += m.v3.xyz;
498                        BarycenterFactor += 1.0f;
499                }
500                if (i == 0)
501                        m_ground_mesh.Render(m);
502                else
503                        m_rigid_mesh[i % 6].Render(m);
504        }
505        if (BarycenterFactor > .0f)
506        {
507                BarycenterLocation /= BarycenterFactor;
508
509                m_camera->SetTarget(BarycenterLocation);
510                m_camera->SetPosition(BarycenterLocation + vec3(-20.0f, 8.0f, .0f));
511        }
512#endif
513}
514
515BtPhysTest::~BtPhysTest()
516{
517        Ticker::Unref(m_camera);
518       
519        while (m_constraint_list.Count())
520        {
521                EasyConstraint* CurPop = m_constraint_list.Last();
522                m_constraint_list.Pop();
523                CurPop->RemoveFromSimulation(m_simulation);
524                delete CurPop;
525        }
526        while (m_ground_list.Count())
527        {
528                PhysicsObject* CurPop = m_ground_list.Last();
529                m_ground_list.Pop();
530                CurPop->GetPhysic()->RemoveFromSimulation(m_simulation);
531                Ticker::Unref(CurPop);
532        }
533        while (m_character_list.Count())
534        {
535                PhysicsObject* CurPop = m_character_list.Last();
536                m_character_list.Pop();
537                CurPop->GetCharacter()->RemoveFromSimulation(m_simulation);
538                Ticker::Unref(CurPop);
539        }
540        while (m_platform_list.Count())
541        {
542                PhysicsObject* CurPop = m_platform_list.Last();
543                m_platform_list.Pop();
544                CurPop->GetPhysic()->RemoveFromSimulation(m_simulation);
545                Ticker::Unref(CurPop);
546        }
547        while (m_physobj_list.Count())
548        {
549                PhysicsObject* CurPop = m_physobj_list.Last();
550                m_physobj_list.Pop();
551                CurPop->GetPhysic()->RemoveFromSimulation(m_simulation);
552                Ticker::Unref(CurPop);
553        }
554    Ticker::Unref(m_simulation);
555
556#if 0
557        //Exit Physics
558        {
559                //cleanup in the reverse order of creation/initialization
560                //remove the rigidbodies from the dynamics world and delete them
561                for (int i = m_bt_world->getNumCollisionObjects() - 1; i >= 0 ;i--)
562                {
563                        btCollisionObject* obj = m_bt_world->getCollisionObjectArray()[i];
564                        btRigidBody* body = btRigidBody::upcast(obj);
565                        if (body && body->getMotionState())
566                                delete body->getMotionState();
567
568                        m_bt_world->removeCollisionObject(obj);
569                        delete obj;
570                }
571
572                //delete collision shapes
573                for (int j = 0; j < m_bt_collision_shapes.Count(); j++)
574                {
575                        btCollisionShape* shape = m_bt_collision_shapes[j];
576                        delete shape;
577                }
578                m_bt_collision_shapes.Empty();
579
580                delete m_bt_world;
581                delete m_bt_solver;
582                delete m_bt_broadphase;
583                delete m_bt_dispatcher;
584                delete m_bt_collision_config;
585        }
586#endif
587}
588
589int main(int argc, char **argv)
590{
591    Application app("BtPhysTest", ivec2(1280, 720), 60.0f);
592
593#if defined _MSC_VER && !defined _XBOX
594    _chdir("..");
595#elif defined _WIN32 && !defined _XBOX
596    _chdir("../..");
597#endif
598
599    new BtPhysTest(argc > 1);
600    app.ShowPointer(false);
601
602    app.Run();
603
604    return EXIT_SUCCESS;
605}
606
Note: See TracBrowser for help on using the repository browser.