← Back to branch summary

~ubuntu-branches/ubuntu/maverick/blender/maverick

« back to all changes in this revision

Viewing changes to source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Khashayar Naderehvandi, Khashayar Naderehvandi, Alessio Treglia
  • Date: 2009-01-22 16:53:59 UTC
  • mfrom: (14.1.1 experimental)
  • Revision ID: james.westby@ubuntu.com-20090122165359-v0996tn7fbit64ni
Tags: 2.48a+dfsg-1ubuntu1
https://launchpad.net/bugs/320045
[ Khashayar Naderehvandi ]
* Merge from debian experimental (LP: #320045), Ubuntu remaining changes:
  - Add patch correcting header file locations.
  - Add libvorbis-dev and libgsm1-dev to Build-Depends.
  - Use avcodec_decode_audio2() in source/blender/src/hddaudio.c

[ Alessio Treglia ]
* Add missing previous changelog entries.

Show diffs side-by-side

added added

removed removed

Lines of Context:
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
23
23
#include "btBulletDynamicsCommon.h"
24
24
#include "LinearMath/btIDebugDraw.h"
25
25
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 
26
#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
 
27
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
 
28
#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
26
29
 
27
30
//profiling/timings
28
31
#include "LinearMath/btQuickprof.h"
30
33
 
31
34
#include "PHY_IMotionState.h"
32
35
 
 
36
#define CCD_CONSTRAINT_DISABLE_LINKED_COLLISION 0x80
33
37
 
34
38
bool useIslands = true;
35
39
 
53
57
 
54
58
 
55
59
#include <stdio.h>
 
60
#include <string.h>             // for memset
56
61
 
57
62
#ifdef NEW_BULLET_VEHICLE_SUPPORT
58
63
class WrapperVehicle : public PHY_IVehicle
329
334
        {
330
335
                m_triggerCallbacks[i] = 0;
331
336
        }
 
337
 
 
338
//      m_collisionConfiguration = new btDefaultCollisionConfiguration();
 
339
        m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
 
340
 
332
341
        if (!dispatcher)
333
342
        {
334
 
                dispatcher = new btCollisionDispatcher();
 
343
                btCollisionDispatcher* disp = new btCollisionDispatcher(m_collisionConfiguration);
 
344
                dispatcher = disp;
 
345
                btGImpactCollisionAlgorithm::registerAlgorithm(disp);
335
346
                m_ownDispatcher = dispatcher;
336
347
        }
337
348
 
338
 
        if(!pairCache)
339
 
        {
340
 
 
341
 
                //todo: calculate/let user specify this world sizes
342
 
                btVector3 worldMin(-10000,-10000,-10000);
343
 
                btVector3 worldMax(10000,10000,10000);
344
 
 
345
 
                pairCache = new btAxisSweep3(worldMin,worldMax);
346
 
                // remember that this was allocated by us so that we can release it
347
 
                m_ownPairCache = pairCache;
348
 
                //broadphase = new btSimpleBroadphase();
349
 
        }
 
349
        //m_broadphase = new btAxisSweep3(btVector3(-1000,-1000,-1000),btVector3(1000,1000,1000));
 
350
        //m_broadphase = new btSimpleBroadphase();
 
351
        m_broadphase = new btDbvtBroadphase();
350
352
 
351
353
        m_filterCallback = new CcdOverlapFilterCallBack(this);
352
 
        pairCache->setOverlapFilterCallback(m_filterCallback);
 
354
        m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback);
353
355
 
354
356
        setSolverType(1);//issues with quickstep and memory allocations
355
 
        m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,m_solver);
 
357
//      m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 
358
        m_dynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 
359
 
356
360
        m_debugDrawer = 0;
357
361
        m_gravity = btVector3(0.f,-10.f,0.f);
358
362
        m_dynamicsWorld->setGravity(m_gravity);
363
367
void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
364
368
{
365
369
        btRigidBody* body = ctrl->GetRigidBody();
 
370
        btCollisionObject* obj = ctrl->GetCollisionObject();
366
371
 
367
372
        //this m_userPointer is just used for triggers, see CallbackTriggers
368
 
        body->setUserPointer(ctrl);
369
 
 
370
 
        body->setGravity( m_gravity );
371
 
        m_controllers.push_back(ctrl);
372
 
 
373
 
        //use explicit group/filter for finer control over collision in bullet => near/radar sensor
374
 
        m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
375
 
        if (body->isStaticOrKinematicObject())
376
 
        {
377
 
                body->setActivationState(ISLAND_SLEEPING);
 
373
        obj->setUserPointer(ctrl);
 
374
        if (body)
 
375
                body->setGravity( m_gravity );
 
376
 
 
377
        m_controllers.insert(ctrl);
 
378
 
 
379
        if (body)
 
380
        {
 
381
                //use explicit group/filter for finer control over collision in bullet => near/radar sensor
 
382
                m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 
383
        } else
 
384
        {
 
385
                if (ctrl->GetSoftBody())
 
386
                {
 
387
                        btSoftBody* softBody = ctrl->GetSoftBody();
 
388
                        m_dynamicsWorld->addSoftBody(softBody);
 
389
                } else
 
390
                {
 
391
                        if (obj->getCollisionShape())
 
392
                        {
 
393
                                m_dynamicsWorld->addCollisionObject(obj,ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 
394
                        }
 
395
                }
 
396
        }
 
397
        if (obj->isStaticOrKinematicObject())
 
398
        {
 
399
                obj->setActivationState(ISLAND_SLEEPING);
378
400
        }
379
401
 
380
402
 
381
403
        //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
382
404
 
383
 
        assert(body->getBroadphaseHandle());
 
405
        assert(obj->getBroadphaseHandle());
384
406
 
385
407
        btBroadphaseInterface* scene =  getBroadphase();
386
408
 
389
411
 
390
412
        assert(shapeinterface);
391
413
 
392
 
        const btTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
 
414
        const btTransform& t = ctrl->GetCollisionObject()->getWorldTransform();
393
415
        
394
416
 
395
417
        btPoint3 minAabb,maxAabb;
401
423
 
402
424
        //extent it with the motion
403
425
 
404
 
        btVector3 linMotion = body->getLinearVelocity()*timeStep;
405
 
 
406
 
        float maxAabbx = maxAabb.getX();
407
 
        float maxAabby = maxAabb.getY();
408
 
        float maxAabbz = maxAabb.getZ();
409
 
        float minAabbx = minAabb.getX();
410
 
        float minAabby = minAabb.getY();
411
 
        float minAabbz = minAabb.getZ();
412
 
 
413
 
        if (linMotion.x() > 0.f)
414
 
                maxAabbx += linMotion.x(); 
415
 
        else
416
 
                minAabbx += linMotion.x();
417
 
        if (linMotion.y() > 0.f)
418
 
                maxAabby += linMotion.y(); 
419
 
        else
420
 
                minAabby += linMotion.y();
421
 
        if (linMotion.z() > 0.f)
422
 
                maxAabbz += linMotion.z(); 
423
 
        else
424
 
                minAabbz += linMotion.z();
425
 
 
426
 
 
427
 
        minAabb = btVector3(minAabbx,minAabby,minAabbz);
428
 
        maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
429
 
 
 
426
        if (body)
 
427
        {
 
428
                btVector3 linMotion = body->getLinearVelocity()*timeStep;
 
429
 
 
430
                float maxAabbx = maxAabb.getX();
 
431
                float maxAabby = maxAabb.getY();
 
432
                float maxAabbz = maxAabb.getZ();
 
433
                float minAabbx = minAabb.getX();
 
434
                float minAabby = minAabb.getY();
 
435
                float minAabbz = minAabb.getZ();
 
436
 
 
437
                if (linMotion.x() > 0.f)
 
438
                        maxAabbx += linMotion.x(); 
 
439
                else
 
440
                        minAabbx += linMotion.x();
 
441
                if (linMotion.y() > 0.f)
 
442
                        maxAabby += linMotion.y(); 
 
443
                else
 
444
                        minAabby += linMotion.y();
 
445
                if (linMotion.z() > 0.f)
 
446
                        maxAabbz += linMotion.z(); 
 
447
                else
 
448
                        minAabbz += linMotion.z();
 
449
 
 
450
 
 
451
                minAabb = btVector3(minAabbx,minAabby,minAabbz);
 
452
                maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
 
453
        }
430
454
 
431
455
 
432
456
 
433
457
}
434
458
 
 
459
                
 
460
 
435
461
void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
436
462
{
437
 
 
438
463
        //also remove constraint
439
 
 
440
 
        
441
 
 
442
 
        m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
443
 
 
444
 
 
445
 
        {
446
 
                std::vector<CcdPhysicsController*>::iterator i =
447
 
                        std::find(m_controllers.begin(), m_controllers.end(), ctrl);
448
 
                if (!(i == m_controllers.end()))
449
 
                {
450
 
                        std::swap(*i, m_controllers.back());
451
 
                        m_controllers.pop_back();
 
464
        btRigidBody* body = ctrl->GetRigidBody();
 
465
        if (body)
 
466
        {
 
467
                m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
 
468
        } else
 
469
        {
 
470
                //if a softbody
 
471
                if (ctrl->GetSoftBody())
 
472
                {
 
473
                        m_dynamicsWorld->removeSoftBody(ctrl->GetSoftBody());
 
474
                } else
 
475
                {
 
476
                        m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
452
477
                }
453
478
        }
 
479
        m_controllers.erase(ctrl);
 
480
 
 
481
        if (ctrl->m_registerCount != 0)
 
482
                printf("Warning: removing controller with non-zero m_registerCount: %d\n", ctrl->m_registerCount);
454
483
 
455
484
        //remove it from the triggers
456
 
        {
457
 
                std::vector<CcdPhysicsController*>::iterator i =
458
 
                        std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
459
 
                if (!(i == m_triggerControllers.end()))
460
 
                {
461
 
                        std::swap(*i, m_triggerControllers.back());
462
 
                        m_triggerControllers.pop_back();
463
 
                }
464
 
        }
465
 
 
466
 
 
 
485
        m_triggerControllers.erase(ctrl);
 
486
}
 
487
 
 
488
void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
 
489
{
 
490
        // this function is used when the collisionning group of a controller is changed
 
491
        // remove and add the collistioning object
 
492
        btRigidBody* body = ctrl->GetRigidBody();
 
493
        btCollisionObject* obj = ctrl->GetCollisionObject();
 
494
        if (obj)
 
495
        {
 
496
                btVector3 inertia(0.0,0.0,0.0);
 
497
                m_dynamicsWorld->removeCollisionObject(obj);
 
498
                obj->setCollisionFlags(newCollisionFlags);
 
499
                if (body)
 
500
                {
 
501
                        if (newMass)
 
502
                                body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
 
503
                        body->setMassProps(newMass, inertia);
 
504
                }
 
505
                m_dynamicsWorld->addCollisionObject(obj, newCollisionGroup, newCollisionMask);
 
506
        }
 
507
        // to avoid nasty interaction, we must update the property of the controller as well
 
508
        ctrl->m_cci.m_mass = newMass;
 
509
        ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
 
510
        ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
 
511
        ctrl->m_cci.m_collisionFlags = newCollisionFlags;
 
512
}
 
513
 
 
514
void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
 
515
{
 
516
        if (m_controllers.insert(ctrl).second)
 
517
        {
 
518
                btCollisionObject* obj = ctrl->GetCollisionObject();
 
519
                obj->setUserPointer(ctrl);
 
520
                m_dynamicsWorld->addCollisionObject(obj, 
 
521
                        ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 
522
        }
 
523
}
 
524
 
 
525
void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ctrl)
 
526
{
 
527
        if (m_controllers.erase(ctrl))
 
528
        {
 
529
                btRigidBody* body = ctrl->GetRigidBody();
 
530
                if (body)
 
531
                {
 
532
                        m_dynamicsWorld->removeRigidBody(body);
 
533
                } else
 
534
                {
 
535
                        if (ctrl->GetSoftBody())
 
536
                        {
 
537
                        } else
 
538
                        {
 
539
                                m_dynamicsWorld->removeCollisionObject(body);
 
540
                        }
 
541
                }
 
542
        }
467
543
}
468
544
 
469
545
 
475
551
 
476
552
bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
477
553
{
 
554
        std::set<CcdPhysicsController*>::iterator it;
 
555
        int i;
478
556
 
479
 
        int i,numCtrl = GetNumControllers();
480
 
        for (i=0;i<numCtrl;i++)
 
557
        for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
481
558
        {
482
 
                CcdPhysicsController* ctrl = GetPhysicsController(i);
483
 
                ctrl->SynchronizeMotionStates(timeStep);
 
559
                (*it)->SynchronizeMotionStates(timeStep);
484
560
        }
485
561
 
 
562
        processFhSprings(curTime,timeStep);
 
563
 
486
564
        float subStep = timeStep / float(m_numTimeSubSteps);
487
565
        for (i=0;i<m_numTimeSubSteps;i++)
488
566
        {
489
 
                m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
490
 
        }
491
 
 
492
 
        numCtrl = GetNumControllers();
493
 
        for (i=0;i<numCtrl;i++)
494
 
        {
495
 
                CcdPhysicsController* ctrl = GetPhysicsController(i);
496
 
                ctrl->SynchronizeMotionStates(timeStep);
 
567
//                      m_dynamicsWorld->stepSimulation(subStep,20,1./240.);//perform always a full simulation step
 
568
                        m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
 
569
        }
 
570
 
 
571
        for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 
572
        {
 
573
                (*it)->SynchronizeMotionStates(timeStep);
 
574
        }
 
575
 
 
576
        for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 
577
        {
 
578
                (*it)->SynchronizeMotionStates(timeStep);
497
579
        }
498
580
 
499
581
        for (i=0;i<m_wrapperVehicles.size();i++)
502
584
                veh->SyncWheels();
503
585
        }
504
586
 
 
587
        if (m_dynamicsWorld->getDebugDrawer() &&  m_dynamicsWorld->getDebugDrawer()->getDebugMode() >0)
 
588
                m_dynamicsWorld->debugDrawWorld();
 
589
 
 
590
 
505
591
        CallbackTriggers();
506
592
 
507
593
        return true;
508
594
}
509
595
 
 
596
class ClosestRayResultCallbackNotMe : public btCollisionWorld::ClosestRayResultCallback
 
597
{
 
598
        btCollisionObject* m_owner;
 
599
        btCollisionObject* m_parent;
 
600
 
 
601
public:
 
602
        ClosestRayResultCallbackNotMe(const btVector3& rayFromWorld,const btVector3& rayToWorld,btCollisionObject* owner,btCollisionObject* parent)
 
603
                :btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld),
 
604
                m_owner(owner)
 
605
        {
 
606
 
 
607
        }
 
608
 
 
609
        virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 
610
        {
 
611
                //don't collide with self
 
612
                if (proxy0->m_clientObject == m_owner)
 
613
                        return false;
 
614
 
 
615
                if (proxy0->m_clientObject == m_parent)
 
616
                        return false;
 
617
 
 
618
                return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
 
619
        }
 
620
 
 
621
};
 
622
 
 
623
void    CcdPhysicsEnvironment::processFhSprings(double curTime,float timeStep)
 
624
{
 
625
        std::set<CcdPhysicsController*>::iterator it;
 
626
        
 
627
        for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 
628
        {
 
629
                CcdPhysicsController* ctrl = (*it);
 
630
                btRigidBody* body = ctrl->GetRigidBody();
 
631
 
 
632
                if (body && (ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh))
 
633
                {
 
634
                        //printf("has Fh or RotFh\n");
 
635
                        //re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
 
636
                        //send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
 
637
 
 
638
                        
 
639
                        CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
 
640
                        btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
 
641
                        btRigidBody* cl_object = parentBody ? parentBody : body;
 
642
 
 
643
                        if (body->isStaticOrKinematicObject())
 
644
                                continue;
 
645
 
 
646
                        btVector3 rayDirLocal(0,0,-10);
 
647
 
 
648
                        //m_dynamicsWorld
 
649
                        //ctrl->GetRigidBody();
 
650
                        btVector3 rayFromWorld = body->getCenterOfMassPosition();
 
651
                        //btVector3     rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
 
652
                        //ray always points down the z axis in world space...
 
653
                        btVector3       rayToWorld = rayFromWorld + rayDirLocal;
 
654
 
 
655
                        ClosestRayResultCallbackNotMe   resultCallback(rayFromWorld,rayToWorld,body,parentBody);
 
656
 
 
657
                        m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
 
658
                        if (resultCallback.hasHit())
 
659
                        {
 
660
                                //we hit this one: resultCallback.m_collisionObject;
 
661
                                CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
 
662
 
 
663
                                if (controller)
 
664
                                {
 
665
                                        if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
 
666
                                                continue;
 
667
 
 
668
                                        btRigidBody* hit_object = controller->GetRigidBody();
 
669
                                        if (!hit_object)
 
670
                                                continue;
 
671
 
 
672
                                        CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
 
673
 
 
674
                                        float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
 
675
                                        if (distance >= hitObjShapeProps.m_fh_distance)
 
676
                                                continue;
 
677
                                        
 
678
                                        
 
679
 
 
680
                                        //btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
 
681
                                        btVector3 ray_dir = rayDirLocal.normalized();
 
682
                                        btVector3 normal = resultCallback.m_hitNormalWorld;
 
683
                                        normal.normalize();
 
684
 
 
685
                                        
 
686
                                        if (ctrl->getConstructionInfo().m_do_fh) 
 
687
                                        {
 
688
                                                btVector3 lspot = cl_object->getCenterOfMassPosition()
 
689
                                                        + rayDirLocal * resultCallback.m_closestHitFraction;
 
690
 
 
691
 
 
692
                                                        
 
693
 
 
694
                                                lspot -= hit_object->getCenterOfMassPosition();
 
695
                                                btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
 
696
                                                btScalar rel_vel_ray = ray_dir.dot(rel_vel);
 
697
                                                btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance; 
 
698
                                                
 
699
                                                btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
 
700
                                                btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
 
701
                                                
 
702
                                                cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir)); 
 
703
                                                if (hitObjShapeProps.m_fh_normal) 
 
704
                                                {
 
705
                                                        cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
 
706
                                                }
 
707
                                                
 
708
                                                btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
 
709
                                                
 
710
                                                
 
711
                                                if (ctrl->getConstructionInfo().m_do_anisotropic) {
 
712
                                                        //Bullet basis contains no scaling/shear etc.
 
713
                                                        const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
 
714
                                                        btVector3 loc_lateral = lateral * lcs;
 
715
                                                        const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
 
716
                                                        loc_lateral *= friction_scaling;
 
717
                                                        lateral = lcs * loc_lateral;
 
718
                                                }
 
719
 
 
720
                                                btScalar rel_vel_lateral = lateral.length();
 
721
                                                
 
722
                                                if (rel_vel_lateral > SIMD_EPSILON) {
 
723
                                                        btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
 
724
 
 
725
                                                        btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
 
726
                                                        
 
727
                                                        btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
 
728
                                                        
 
729
                                                        btVector3 friction = (rel_mom_lateral > max_friction) ?
 
730
                                                                -lateral * (max_friction / rel_vel_lateral) :
 
731
                                                                -lateral;
 
732
                                                        
 
733
                                                                cl_object->applyCentralImpulse(friction);
 
734
                                                }
 
735
                                        }
 
736
 
 
737
                                        
 
738
                                        if (ctrl->getConstructionInfo().m_do_rot_fh) {
 
739
                                                btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
 
740
 
 
741
                                                btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
 
742
                                                btVector3 ang_vel = cl_object->getAngularVelocity();
 
743
                                                
 
744
                                                // only rotations that tilt relative to the normal are damped
 
745
                                                ang_vel -= ang_vel.dot(normal) * normal;
 
746
                                                
 
747
                                                btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;  
 
748
                                                
 
749
                                                cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
 
750
                                        }
 
751
 
 
752
                                }
 
753
 
 
754
 
 
755
                        }
 
756
 
 
757
 
 
758
                }
 
759
        }
 
760
        
 
761
}
510
762
 
511
763
void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
512
764
{
562
814
 
563
815
void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
564
816
{
565
 
        gLinearAirDamping = damping;
 
817
        //gLinearAirDamping = damping;
566
818
}
567
819
 
568
820
void            CcdPhysicsEnvironment::setUseEpa(bool epa)
581
833
                        {
582
834
 
583
835
                                m_solver = new btSequentialImpulseConstraintSolver();
584
 
                                ((btSequentialImpulseConstraintSolver*)m_solver)->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
 
836
//                              ((btSequentialImpulseConstraintSolver*)m_solver)->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
585
837
                                break;
586
838
                        }
587
839
                }
602
854
 
603
855
 
604
856
 
605
 
 
 
857
void            CcdPhysicsEnvironment::getGravity(PHY__Vector3& grav)
 
858
{
 
859
                const btVector3& gravity = m_dynamicsWorld->getGravity();
 
860
                grav[0] = gravity.getX();
 
861
                grav[1] = gravity.getY();
 
862
                grav[2] = gravity.getZ();
 
863
}
606
864
 
607
865
 
608
866
void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
625
883
                                                const btVector3& linearMinLimits,
626
884
                                                const btVector3& linearMaxLimits,
627
885
                                                const btVector3& angularMinLimits,
628
 
                                                const btVector3& angularMaxLimits
 
886
                                                const btVector3& angularMaxLimits,int flags
629
887
)
630
888
{
631
889
 
 
890
        bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
 
891
 
632
892
        //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
633
893
        //perhaps some warning or hint that hinge/ball-socket is more efficient?
634
894
        
 
895
        
635
896
        btGeneric6DofConstraint* genericConstraint = 0;
636
897
        CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
637
898
        CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
643
904
        {
644
905
                
645
906
 
 
907
                bool useReferenceFrameA = true;
646
908
                genericConstraint = new btGeneric6DofConstraint(
647
909
                        *rb0,*rb1,
648
 
                        frameInA,frameInB);
 
910
                        frameInA,frameInB,useReferenceFrameA);
649
911
                genericConstraint->setLinearLowerLimit(linearMinLimits);
650
912
                genericConstraint->setLinearUpperLimit(linearMaxLimits);
651
913
                genericConstraint->setAngularLowerLimit(angularMinLimits);
660
922
        if (genericConstraint)
661
923
        {
662
924
        //      m_constraints.push_back(genericConstraint);
663
 
                m_dynamicsWorld->addConstraint(genericConstraint);
 
925
                m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
664
926
 
665
927
                genericConstraint->setUserConstraintId(gConstraintUid++);
666
928
                genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
693
955
 
694
956
struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
695
957
{
696
 
        PHY_IPhysicsController* m_ignoreClient;
 
958
        PHY_IRayCastFilterCallback&     m_phyRayFilter;
 
959
        const btCollisionShape*         m_hitTriangleShape;
 
960
        int                                                     m_hitTriangleIndex;
697
961
 
698
 
        FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const btVector3& rayFrom,const btVector3& rayTo)
 
962
        FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
699
963
                : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
700
 
                m_ignoreClient(ignoreClient)
 
964
                m_phyRayFilter(phyRayFilter),
 
965
                m_hitTriangleShape(NULL),
 
966
                m_hitTriangleIndex(0)
701
967
        {
702
 
 
703
968
        }
704
969
 
705
970
        virtual ~FilterClosestRayResultCallback()
706
971
        {
707
972
        }
708
973
 
709
 
        virtual float   AddSingleResult( btCollisionWorld::LocalRayResult& rayResult)
 
974
        virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 
975
        {
 
976
                if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
 
977
                        return false;
 
978
                if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
 
979
                        return false;
 
980
                btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
 
981
                CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
 
982
                if (phyCtrl == m_phyRayFilter.m_ignoreController)
 
983
                        return false;
 
984
                return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
 
985
        }
 
986
 
 
987
        virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
710
988
        {
711
989
                CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
712
 
                //ignore client...
713
 
                if (curHit != m_ignoreClient)
714
 
                {               
715
 
                        //if valid
716
 
                        return ClosestRayResultCallback::AddSingleResult(rayResult);
 
990
                // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
 
991
                if (rayResult.m_localShapeInfo)
 
992
                {
 
993
                        m_hitTriangleShape = rayResult.m_collisionObject->getCollisionShape();
 
994
                        m_hitTriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
 
995
                } else 
 
996
                {
 
997
                        m_hitTriangleShape = NULL;
 
998
                        m_hitTriangleIndex = 0;
717
999
                }
718
 
                return m_closestHitFraction;
 
1000
                return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
719
1001
        }
720
1002
 
721
1003
};
722
1004
 
723
 
PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
724
 
                                                                                                           float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 
1005
PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
725
1006
{
726
1007
 
727
1008
 
735
1016
        //Either Ray Cast with or without filtering
736
1017
 
737
1018
        //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
738
 
        FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
739
 
 
740
 
 
741
 
        PHY_IPhysicsController* nearestHit = 0;
 
1019
        FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
 
1020
 
 
1021
 
 
1022
        PHY_RayCastResult result;
 
1023
        memset(&result, 0, sizeof(result));
 
1024
 
742
1025
        // don't collision with sensor object
743
 
        m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback, CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter);
744
 
        if (rayCallback.HasHit())
 
1026
        rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
 
1027
        //, ,filterCallback.m_faceNormal);
 
1028
 
 
1029
        m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
 
1030
        if (rayCallback.hasHit())
745
1031
        {
746
 
                nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
747
 
                hitX =  rayCallback.m_hitPointWorld.getX();
748
 
                hitY =  rayCallback.m_hitPointWorld.getY();
749
 
                hitZ =  rayCallback.m_hitPointWorld.getZ();
750
 
 
 
1032
                CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
 
1033
                result.m_controller = controller;
 
1034
                result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
 
1035
                result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
 
1036
                result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
 
1037
 
 
1038
                if (rayCallback.m_hitTriangleShape != NULL)
 
1039
                {
 
1040
                        // identify the mesh polygon
 
1041
                        CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
 
1042
                        if (shapeInfo)
 
1043
                        {
 
1044
                                btCollisionShape* shape = controller->GetCollisionObject()->getCollisionShape();
 
1045
                                if (shape->isCompound())
 
1046
                                {
 
1047
                                        btCompoundShape* compoundShape = (btCompoundShape*)shape;
 
1048
                                        CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
 
1049
                                        // need to search which sub-shape has been hit
 
1050
                                        for (int i=0; i<compoundShape->getNumChildShapes(); i++)
 
1051
                                        {
 
1052
                                                shapeInfo = compoundShapeInfo->GetChildShape(i);
 
1053
                                                shape=compoundShape->getChildShape(i);
 
1054
                                                if (shape == rayCallback.m_hitTriangleShape)
 
1055
                                                        break;
 
1056
                                        }
 
1057
                                }
 
1058
                                if (shape == rayCallback.m_hitTriangleShape && 
 
1059
                                        rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
 
1060
                                {
 
1061
                                        result.m_meshObject = shapeInfo->GetMesh();
 
1062
                                        result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
 
1063
 
 
1064
                                        // Bullet returns the normal from "outside".
 
1065
                                        // If the user requests the real normal, compute it now
 
1066
                    if (filterCallback.m_faceNormal)
 
1067
                                        {
 
1068
                                                // mesh shapes are shared and stored in the shapeInfo
 
1069
                                                btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
 
1070
                                                if (triangleShape)
 
1071
                                                {
 
1072
                                                        // this code is copied from Bullet 
 
1073
                                                        btVector3 triangle[3];
 
1074
                                                        const unsigned char *vertexbase;
 
1075
                                                        int numverts;
 
1076
                                                        PHY_ScalarType type;
 
1077
                                                        int stride;
 
1078
                                                        const unsigned char *indexbase;
 
1079
                                                        int indexstride;
 
1080
                                                        int numfaces;
 
1081
                                                        PHY_ScalarType indicestype;
 
1082
                                                        btStridingMeshInterface* meshInterface = triangleShape->getMeshInterface();
 
1083
 
 
1084
                                                        meshInterface->getLockedReadOnlyVertexIndexBase(
 
1085
                                                                &vertexbase,
 
1086
                                                                numverts,
 
1087
                                                                type,
 
1088
                                                                stride,
 
1089
                                                                &indexbase,
 
1090
                                                                indexstride,
 
1091
                                                                numfaces,
 
1092
                                                                indicestype,
 
1093
                                                                0);
 
1094
 
 
1095
                                                        unsigned int* gfxbase = (unsigned int*)(indexbase+rayCallback.m_hitTriangleIndex*indexstride);
 
1096
                                                        const btVector3& meshScaling = shape->getLocalScaling();
 
1097
                                                        for (int j=2;j>=0;j--)
 
1098
                                                        {
 
1099
                                                                int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
 
1100
 
 
1101
                                                                btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
 
1102
 
 
1103
                                                                triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());              
 
1104
                                                        }
 
1105
                                                        meshInterface->unLockReadOnlyVertexBase(0);
 
1106
                                                        btVector3 triangleNormal; 
 
1107
                                                        triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
 
1108
                                                        rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
 
1109
                                                }
 
1110
                                        }
 
1111
                                }
 
1112
                        }
 
1113
                }
751
1114
                if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
752
1115
                {
753
1116
                        rayCallback.m_hitNormalWorld.normalize();
755
1118
                {
756
1119
                        rayCallback.m_hitNormalWorld.setValue(1,0,0);
757
1120
                }
758
 
                normalX = rayCallback.m_hitNormalWorld.getX();
759
 
                normalY = rayCallback.m_hitNormalWorld.getY();
760
 
                normalZ = rayCallback.m_hitNormalWorld.getZ();
761
 
 
 
1121
                result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
 
1122
                result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
 
1123
                result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
 
1124
                filterCallback.reportHit(&result);
762
1125
        }       
763
1126
 
764
1127
 
765
 
        return nearestHit;
 
1128
        return result.m_controller;
766
1129
}
767
1130
 
768
1131
 
785
1148
        return m_dynamicsWorld->getBroadphase(); 
786
1149
}
787
1150
 
 
1151
btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
 
1152
{ 
 
1153
        return m_dynamicsWorld->getDispatcher();
 
1154
}
 
1155
 
 
1156
 
 
1157
 
788
1158
 
789
1159
 
790
1160
 
817
1187
 
818
1188
        if (NULL != m_filterCallback)
819
1189
                delete m_filterCallback;
820
 
}
821
 
 
822
 
 
823
 
int     CcdPhysicsEnvironment::GetNumControllers()
824
 
{
825
 
        return m_controllers.size();
826
 
}
827
 
 
828
 
 
829
 
CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
830
 
{
831
 
        return m_controllers[index];
832
 
}
833
 
 
834
 
 
 
1190
 
 
1191
        if (NULL != m_collisionConfiguration)
 
1192
                delete m_collisionConfiguration;
 
1193
 
 
1194
        if (NULL != m_broadphase)
 
1195
                delete m_broadphase;
 
1196
}
835
1197
 
836
1198
 
837
1199
void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
843
1205
                {
844
1206
                        //param = 1..12, min0,max0,min1,max1...min6,max6
845
1207
                        btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
846
 
                        genCons->SetLimit(param,value0,value1);
 
1208
                        genCons->setLimit(param,value0,value1);
847
1209
                        break;
848
1210
                };
849
1211
        default:
873
1235
{
874
1236
 
875
1237
        CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
876
 
        std::vector<CcdPhysicsController*>::iterator i =
877
 
                std::find(m_controllers.begin(), m_controllers.end(), ctrl);
878
 
        if ((i == m_controllers.end()))
879
 
        {
880
 
                addCcdPhysicsController(ctrl1);
881
 
        }
 
1238
        // addSensor() is a "light" function for bullet because it is used
 
1239
        // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead 
 
1240
        //if (m_controllers.insert(ctrl1).second)
 
1241
        //{
 
1242
        //      addCcdPhysicsController(ctrl1);
 
1243
        //}
 
1244
        enableCcdPhysicsController(ctrl1);
 
1245
 
882
1246
        //Collision filter/mask is now set at the time of the creation of the controller 
883
1247
        //force collision detection with everything, including static objects (might hurt performance!)
884
1248
        //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
891
1255
 
892
1256
void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
893
1257
{
894
 
        std::vector<CcdPhysicsController*>::iterator i =
895
 
                std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
896
 
        if (!(i == m_triggerControllers.end()))
897
 
        {
898
 
                std::swap(*i, m_triggerControllers.back());
899
 
                m_triggerControllers.pop_back();
900
 
        }
 
1258
        CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
 
1259
        if (ccdCtrl->Unregister())
 
1260
                m_triggerControllers.erase(ccdCtrl);
901
1261
}
902
1262
 
903
1263
 
904
1264
void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
905
1265
{
906
1266
        removeCollisionCallback(ctrl);
907
 
        //printf("removeSensor\n");
 
1267
 
 
1268
        disableCcdPhysicsController((CcdPhysicsController*)ctrl);
908
1269
}
 
1270
 
909
1271
void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
910
1272
{
911
1273
        /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
942
1304
{
943
1305
        CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
944
1306
 
945
 
        //printf("requestCollisionCallback\n");
946
 
        m_triggerControllers.push_back(ccdCtrl);
 
1307
        if (ccdCtrl->Register())
 
1308
                m_triggerControllers.insert(ccdCtrl);
947
1309
}
948
1310
 
949
 
 
950
1311
void    CcdPhysicsEnvironment::CallbackTriggers()
951
1312
{
952
1313
        
955
1316
        if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
956
1317
        {
957
1318
                //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
958
 
                int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
 
1319
                btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
 
1320
                int numManifolds = dispatcher->getNumManifolds();
959
1321
                for (int i=0;i<numManifolds;i++)
960
1322
                {
961
 
                        btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
 
1323
                        btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
962
1324
                        int numContacts = manifold->getNumContacts();
963
1325
                        if (numContacts)
964
1326
                        {
 
1327
                                btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
 
1328
                                btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
965
1329
                                if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
966
1330
                                {
967
1331
                                        for (int j=0;j<numContacts;j++)
972
1336
                                                        m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
973
1337
                                        }
974
1338
                                }
975
 
                                btRigidBody* obj0 = static_cast<btRigidBody* >(manifold->getBody0());
976
 
                                btRigidBody* obj1 = static_cast<btRigidBody* >(manifold->getBody1());
 
1339
                                btRigidBody* obj0 = rb0;
 
1340
                                btRigidBody* obj1 = rb1;
977
1341
 
978
1342
                                //m_internalOwner is set in 'addPhysicsController'
979
1343
                                CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
980
1344
                                CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
981
1345
 
982
 
                                std::vector<CcdPhysicsController*>::iterator i =
983
 
                                        std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
 
1346
                                std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
984
1347
                                if (i == m_triggerControllers.end())
985
1348
                                {
986
 
                                        i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
 
1349
                                        i = m_triggerControllers.find(ctrl1);
987
1350
                                }
988
1351
 
989
1352
                                if (!(i == m_triggerControllers.end()))
991
1354
                                        m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
992
1355
                                                ctrl0,ctrl1,0);
993
1356
                                }
 
1357
                                // Bullet does not refresh the manifold contact point for object without contact response
 
1358
                                // may need to remove this when a newer Bullet version is integrated
 
1359
                                if (!dispatcher->needsResponse(rb0, rb1))
 
1360
                                {
 
1361
                                        // Refresh algorithm fails sometimes when there is penetration 
 
1362
                                        // (usuall the case with ghost and sensor objects)
 
1363
                                        // Let's just clear the manifold, in any case, it is recomputed on each frame.
 
1364
                                        manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 
1365
                                }
994
1366
                        }
995
1367
                }
996
1368
 
1093
1465
 
1094
1466
        CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1095
1467
        
1096
 
 
1097
1468
        return sphereController;
1098
1469
}
1099
1470
 
 
1471
int findClosestNode(btSoftBody* sb,const btVector3& worldPoint);
 
1472
int findClosestNode(btSoftBody* sb,const btVector3& worldPoint)
 
1473
{
 
1474
        int node = -1;
 
1475
 
 
1476
        btSoftBody::tNodeArray&   nodes(sb->m_nodes);
 
1477
        float maxDistSqr = 1e30f;
 
1478
 
 
1479
        for (int n=0;n<nodes.size();n++)
 
1480
        {
 
1481
                btScalar distSqr = (nodes[n].m_x - worldPoint).length2();
 
1482
                if (distSqr<maxDistSqr)
 
1483
                {
 
1484
                        maxDistSqr = distSqr;
 
1485
                        node = n;
 
1486
                }
 
1487
        }
 
1488
        return node;
 
1489
}
 
1490
 
1100
1491
int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1101
1492
                                                                                                        float pivotX,float pivotY,float pivotZ,
1102
1493
                                                                                                        float axisX,float axisY,float axisZ,
1103
1494
                                                                                                        float axis1X,float axis1Y,float axis1Z,
1104
 
                                                                                                        float axis2X,float axis2Y,float axis2Z
 
1495
                                                                                                        float axis2X,float axis2Y,float axis2Z,int flags
1105
1496
                                                                                                        )
1106
1497
{
1107
1498
 
 
1499
        bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
 
1500
 
 
1501
 
1108
1502
 
1109
1503
        CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1110
1504
        CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1112
1506
        btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1113
1507
        btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1114
1508
 
 
1509
        
 
1510
 
 
1511
 
1115
1512
        bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1116
1513
        bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
 
1514
 
 
1515
        btCollisionObject* colObj0 = c0->GetCollisionObject();
 
1516
        if (!colObj0)
 
1517
        {
 
1518
                return 0;
 
1519
        }
 
1520
 
 
1521
        btVector3 pivotInA(pivotX,pivotY,pivotZ);
 
1522
 
1117
1523
        
1118
1524
 
 
1525
        //it might be a soft body, let's try
 
1526
        btSoftBody* sb0 = c0 ? c0->GetSoftBody() : 0;
 
1527
        btSoftBody* sb1 = c1 ? c1->GetSoftBody() : 0;
 
1528
        if (sb0 && sb1)
 
1529
        {
 
1530
                //not between two soft bodies?
 
1531
                return 0;
 
1532
        }
 
1533
 
 
1534
        if (sb0)
 
1535
        {
 
1536
                //either cluster or node attach, let's find closest node first
 
1537
                //the soft body doesn't have a 'real' world transform, so get its initial world transform for now
 
1538
                btVector3 pivotPointSoftWorld = sb0->m_initialWorldTransform(pivotInA);
 
1539
                int node=findClosestNode(sb0,pivotPointSoftWorld);
 
1540
                if (node >=0)
 
1541
                {
 
1542
                        bool clusterconstaint = false;
 
1543
/*
 
1544
                        switch (type)
 
1545
                        {
 
1546
                        case PHY_LINEHINGE_CONSTRAINT:
 
1547
                                {
 
1548
                                        if (sb0->clusterCount() && rb1)
 
1549
                                        {
 
1550
                                                btSoftBody::LJoint::Specs       ls;
 
1551
                                                ls.erp=0.5f;
 
1552
                                                ls.position=sb0->clusterCom(0);
 
1553
                                                sb0->appendLinearJoint(ls,rb1);
 
1554
                                                clusterconstaint = true;
 
1555
                                                break;
 
1556
                                        }
 
1557
                                }
 
1558
                        case PHY_GENERIC_6DOF_CONSTRAINT:
 
1559
                                {
 
1560
                                        if (sb0->clusterCount() && rb1)
 
1561
                                        {
 
1562
                                                btSoftBody::AJoint::Specs as;
 
1563
                                                as.erp = 1;
 
1564
                                                as.cfm = 1;
 
1565
                                                as.axis.setValue(axisX,axisY,axisZ);
 
1566
                                                sb0->appendAngularJoint(as,rb1);
 
1567
                                                clusterconstaint = true;
 
1568
                                                break;
 
1569
                                        }
 
1570
 
 
1571
                                        break;
 
1572
                                }
 
1573
                        default:
 
1574
                                {
 
1575
                                
 
1576
                                }
 
1577
                        };
 
1578
                        */
 
1579
 
 
1580
                        if (!clusterconstaint)
 
1581
                        {
 
1582
                                if (rb1)
 
1583
                                {
 
1584
                                        sb0->appendAnchor(node,rb1,disableCollisionBetweenLinkedBodies);
 
1585
                                } else
 
1586
                                {
 
1587
                                        sb0->setMass(node,0.f);
 
1588
                                }
 
1589
                        }
 
1590
 
 
1591
                        
 
1592
                }
 
1593
                return 0;//can't remove soft body anchors yet
 
1594
        }
 
1595
 
 
1596
        if (sb1)
 
1597
        {
 
1598
                btVector3 pivotPointAWorld = colObj0->getWorldTransform()(pivotInA);
 
1599
                int node=findClosestNode(sb1,pivotPointAWorld);
 
1600
                if (node >=0)
 
1601
                {
 
1602
                        bool clusterconstaint = false;
 
1603
 
 
1604
                        /*
 
1605
                        switch (type)
 
1606
                        {
 
1607
                        case PHY_LINEHINGE_CONSTRAINT:
 
1608
                                {
 
1609
                                        if (sb1->clusterCount() && rb0)
 
1610
                                        {
 
1611
                                                btSoftBody::LJoint::Specs       ls;
 
1612
                                                ls.erp=0.5f;
 
1613
                                                ls.position=sb1->clusterCom(0);
 
1614
                                                sb1->appendLinearJoint(ls,rb0);
 
1615
                                                clusterconstaint = true;
 
1616
                                                break;
 
1617
                                        }
 
1618
                                }
 
1619
                        case PHY_GENERIC_6DOF_CONSTRAINT:
 
1620
                                {
 
1621
                                        if (sb1->clusterCount() && rb0)
 
1622
                                        {
 
1623
                                                btSoftBody::AJoint::Specs as;
 
1624
                                                as.erp = 1;
 
1625
                                                as.cfm = 1;
 
1626
                                                as.axis.setValue(axisX,axisY,axisZ);
 
1627
                                                sb1->appendAngularJoint(as,rb0);
 
1628
                                                clusterconstaint = true;
 
1629
                                                break;
 
1630
                                        }
 
1631
 
 
1632
                                        break;
 
1633
                                }
 
1634
                        default:
 
1635
                                {
 
1636
                                        
 
1637
 
 
1638
                                }
 
1639
                        };*/
 
1640
 
 
1641
 
 
1642
                        if (!clusterconstaint)
 
1643
                        {
 
1644
                                if (rb0)
 
1645
                                {
 
1646
                                        sb1->appendAnchor(node,rb0,disableCollisionBetweenLinkedBodies);
 
1647
                                } else
 
1648
                                {
 
1649
                                        sb1->setMass(node,0.f);
 
1650
                                }
 
1651
                        }
 
1652
                        
 
1653
 
 
1654
                }
 
1655
                return 0;//can't remove soft body anchors yet
 
1656
        }
 
1657
 
1119
1658
        if (rb0static && rb1static)
1120
 
                return 0;
1121
 
 
1122
 
        btVector3 pivotInA(pivotX,pivotY,pivotZ);
 
1659
        {
 
1660
                
 
1661
                return 0;
 
1662
        }
 
1663
        
 
1664
 
 
1665
        if (!rb0)
 
1666
                return 0;
 
1667
 
 
1668
        
1123
1669
        btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1124
1670
                rb0->getCenterOfMassTransform() * pivotInA;
1125
1671
        btVector3 axisInA(axisX,axisY,axisZ);
1144
1690
                                        pivotInA);
1145
1691
                        }
1146
1692
 
1147
 
                        m_dynamicsWorld->addConstraint(p2p);
 
1693
                        m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
1148
1694
//                      m_constraints.push_back(p2p);
1149
1695
 
1150
1696
                        p2p->setUserConstraintId(gConstraintUid++);
1180
1726
                                btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1181
1727
                                
1182
1728
                                frameInB = inv  * globalFrameA;
1183
 
                                
 
1729
                                bool useReferenceFrameA = true;
 
1730
 
1184
1731
                                genericConstraint = new btGeneric6DofConstraint(
1185
1732
                                        *rb0,*rb1,
1186
 
                                        frameInA,frameInB);
 
1733
                                        frameInA,frameInB,useReferenceFrameA);
1187
1734
 
1188
1735
 
1189
1736
                        } else
1204
1751
                                ///frameInB in worldspace
1205
1752
                                frameInB = rb0->getCenterOfMassTransform() * frameInA;
1206
1753
 
 
1754
                                bool useReferenceFrameA = true;
1207
1755
                                genericConstraint = new btGeneric6DofConstraint(
1208
1756
                                        *rb0,s_fixedObject2,
1209
 
                                        frameInA,frameInB);
 
1757
                                        frameInA,frameInB,useReferenceFrameA);
1210
1758
                        }
1211
1759
                        
1212
1760
                        if (genericConstraint)
1213
1761
                        {
1214
1762
                                //m_constraints.push_back(genericConstraint);
1215
 
                                m_dynamicsWorld->addConstraint(genericConstraint);
 
1763
                                m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
1216
1764
                                genericConstraint->setUserConstraintId(gConstraintUid++);
1217
1765
                                genericConstraint->setUserConstraintType(type);
1218
1766
                                //64 bit systems can't cast pointer to int. could use size_t instead.
1278
1826
                        if (coneTwistContraint)
1279
1827
                        {
1280
1828
                                //m_constraints.push_back(genericConstraint);
1281
 
                                m_dynamicsWorld->addConstraint(coneTwistContraint);
 
1829
                                m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
1282
1830
                                coneTwistContraint->setUserConstraintId(gConstraintUid++);
1283
1831
                                coneTwistContraint->setUserConstraintType(type);
1284
1832
                                //64 bit systems can't cast pointer to int. could use size_t instead.
1317
1865
                        hinge->setAngularOnly(angularOnly);
1318
1866
 
1319
1867
                        //m_constraints.push_back(hinge);
1320
 
                        m_dynamicsWorld->addConstraint(hinge);
 
1868
                        m_dynamicsWorld->addConstraint(hinge,disableCollisionBetweenLinkedBodies);
1321
1869
                        hinge->setUserConstraintId(gConstraintUid++);
1322
1870
                        hinge->setUserConstraintType(type);
1323
1871
                        //64 bit systems can't cast pointer to int. could use size_t instead.
1359
1907
PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1360
1908
{
1361
1909
        CcdConstructionInfo     cinfo;
1362
 
        //This is a memory leak: Bullet does not delete the shape and it cannot be added to 
1363
 
        //the KX_Scene.m_shapes list -- too bad but that's not a lot of data
 
1910
 
 
1911
        // we don't need a CcdShapeConstructionInfo for this shape:
 
1912
        // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
1364
1913
        cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1365
1914
        cinfo.m_MotionState = 0;
1366
1915
        cinfo.m_physicsEnv = this;