source: trunk/src/bullet/bullet_world.cc @ 520

// Copyright (C) 2016 ChaosForge Ltd
// http://chaosforge.org/
//
// This file is part of Nova libraries. 
// For conditions of distribution and use, see copying.txt file in root folder.

#include "nv/bullet/bullet_world.hh"

#include "src/bullet/bullet_helper.hh"
#include <btBulletDynamicsCommon.h>
#include <btBulletCollisionCommon.h>

using namespace nv;

struct alignas( 16 ) motion_state : public btMotionState
{
        btTransform m_gfx_world_tr;
        btTransform     m_com_offset;
        btTransform     m_com_offset_inv;
        SIMD_FORCE_INLINE void* operator new( size_t sz ) { return btAlignedAlloc( sz, 16 ); }
        SIMD_FORCE_INLINE void  operator delete( void* ptr ) { btAlignedFree( ptr ); }

        motion_state( const btTransform& start_tr = btTransform::getIdentity(), const btTransform& com_offset = btTransform::getIdentity() )
                : m_gfx_world_tr( start_tr )
                , m_com_offset( com_offset )
                , m_com_offset_inv( m_com_offset.inverse() )
        {
        }

        motion_state( const transform& startTrans = transform(), const nv::transform& com_offset = nv::transform() )
                : m_gfx_world_tr( n2b( startTrans ) )
                , m_com_offset( n2b( com_offset ) )
                , m_com_offset_inv( m_com_offset.inverse() )
        {
        }

        nv::transform get_world_transform() const
        {
                return b2n( m_gfx_world_tr );
        }

        void set_world_transform( const nv::transform& tr )
        {
                m_gfx_world_tr = n2b( tr );
        }

        virtual void    getWorldTransform( btTransform& com_world_tr ) const
        {
                com_world_tr = m_gfx_world_tr * m_com_offset_inv;
        }

        virtual void    setWorldTransform( const btTransform& com_world_tr )
        {
                m_gfx_world_tr = com_world_tr * m_com_offset;
        }

};


#define SELF static_cast<btDiscreteDynamicsWorld*>( world )

bullet_world::bullet_world()
{
        btBroadphaseInterface*               broadphase = new btDbvtBroadphase();
        btDefaultCollisionConfiguration*     collisionConfiguration = new btDefaultCollisionConfiguration();
        btCollisionDispatcher*               dispatcher = new btCollisionDispatcher( collisionConfiguration );
        btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
        btDiscreteDynamicsWorld*             dworld = new btDiscreteDynamicsWorld( dispatcher, broadphase, solver, collisionConfiguration );
        world = dworld;
}

void bullet_world::set_gravity( const vec3& gravity )
{
        SELF->setGravity( n2b( gravity ) );
}

int nv::bullet_world::step_simulation( float dtime, int max_sub_steps /*= 1*/, float fixed_time_step /*= 1.0f / 60.0f */ )
{
        return SELF->stepSimulation( dtime, max_sub_steps, fixed_time_step );
}

collision_shape bullet_world::create_sphere( float radius )
{
        return collision_shape{ ( void* )new btSphereShape( radius ) };
}

nv::collision_shape nv::bullet_world::create_capsule( float radius, float height )
{
        return collision_shape{ ( void* )new btCapsuleShape( radius, height ) };
}

collision_shape bullet_world::create_box( const vec3& half_extens )
{
        return collision_shape{ ( void* )new btBoxShape( n2b( half_extens ) ) };
}

collision_shape bullet_world::create_static_plane( const vec3& norm, float cst )
{
        return collision_shape{ ( void* )new btStaticPlaneShape( n2b( norm ), cst ) };
}

void bullet_world::add_rigid_body( rigid_body body )
{
        SELF->addRigidBody( static_cast<btRigidBody*>( body.internal ) );
}

rigid_body bullet_world::create_rigid_body( float mass, const transform& tr, collision_shape shape, const vec3& com_offset )
{
        btCollisionShape* sh = static_cast<btCollisionShape*>( shape.internal );
        bool is_dynamic = ( mass != 0.f );

        btVector3 local_inertia( 0, 0, 0 );
        if ( is_dynamic )
                sh->calculateLocalInertia( mass, local_inertia );

        motion_state* ms = new motion_state( tr, nv::transform( com_offset ) );

        btRigidBody::btRigidBodyConstructionInfo rb_info( mass, ms, sh, local_inertia );
        btRigidBody* body = new btRigidBody( rb_info );
        return rigid_body{ body };
}

nv::collision_shape nv::bullet_world::create_mesh( array_view< vec3 > vtx, array_view< int > idx )
{
        btTriangleMesh* mesh = new btTriangleMesh;
        for ( uint32 i = 0; i < idx.size() / 3; ++i )
        {
                mesh->addTriangle( n2b( vtx[idx[i * 3+0]] ), n2b(vtx[idx[i * 3+1]]), n2b( vtx[idx[i * 3+ 2]] ) );
        }

//      btTriangleIndexVertexArray* array = new btTriangleIndexVertexArray( idx.size() / 3, (int*)idx.data(), 3 * sizeof(int), vtx.size(), (float*)vtx.data(), sizeof(vtx) );
        btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape( mesh, true );
//      shape->buildOptimizedBvh();
        return collision_shape{ shape, nullptr };
}

constraint nv::bullet_world::create_hinge_constraint( rigid_body a, const transform& ta, rigid_body b, const transform& tb, const vec2& low_high, const vec3& params /*= vec3( 0.9f, 0.3f, 1.0f ) */ )
{
        btRigidBody* rba = static_cast<btRigidBody*>( a.internal );
        btRigidBody* rbb = static_cast<btRigidBody*>( b.internal );
        btHingeConstraint* hinge = new btHingeConstraint( *rba, *rbb, n2b( ta ), n2b( tb ) );
        hinge->setLimit( low_high.x, low_high.y, params.x, params.y, params.z );
        return constraint{ hinge };
}

constraint nv::bullet_world::create_cone_twist_constraint( rigid_body a, const transform& ta, rigid_body b, const transform& tb, const vec3& sst, const vec3& params /*= vec3( 1.0f, 0.3f, 1.0f ) */ )
{
        btRigidBody* rba = static_cast<btRigidBody*>( a.internal );
        btRigidBody* rbb = static_cast<btRigidBody*>( b.internal );
        btConeTwistConstraint* cone_twist = new btConeTwistConstraint( *rba, *rbb, n2b( ta ), n2b( tb ) );
        cone_twist->setLimit( sst.x, sst.y, sst.z, params.x, params.y, params.z );
        return constraint{ cone_twist };
}

void nv::bullet_world::remove_rigid_body( rigid_body body )
{
        SELF->removeRigidBody( static_cast<btRigidBody*>( body.internal ) );
}

void nv::bullet_world::add_constraint( constraint cons )
{
        SELF->addConstraint( static_cast<btTypedConstraint*>( cons.internal ), true );
}

void nv::bullet_world::set_rigid_body_damping( rigid_body body, float linear, float angular )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        rb->setDamping( linear, angular );
}

void nv::bullet_world::set_rigid_body_deactivation_time( rigid_body body, float time )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        rb->setDeactivationTime( time );
}

void nv::bullet_world::set_rigid_body_activation_state( rigid_body body, activation_state state )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        rb->setActivationState( state );
}

void nv::bullet_world::set_rigid_body_sleeping_thresholds( rigid_body body, float linear, float angular )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        rb->setSleepingThresholds( linear, angular );
}

void nv::bullet_world::set_rigid_body_linear_velocity( rigid_body body, const vec3& velocity )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        rb->setLinearVelocity( n2b( velocity ) );
}

void nv::bullet_world::set_rigid_body_ccd( rigid_body body, float radius, float threshold )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        rb->setCcdMotionThreshold( threshold );
        rb->setCcdSweptSphereRadius( radius );
}

nv::transform nv::bullet_world::get_world_transform( rigid_body body )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        return static_cast<motion_state*>( rb->getMotionState() )->get_world_transform();
}

void nv::bullet_world::set_world_transform( rigid_body body, const nv::transform& tr )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        static_cast<motion_state*>( rb->getMotionState() )->set_world_transform( tr );
}

int nv::bullet_world::get_collision_flags( rigid_body body )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        return rb->getCollisionFlags();
}

void nv::bullet_world::set_collision_flags( rigid_body body, int flags )
{
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        return rb->setCollisionFlags( flags );
}

class ClosestStaticRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
public:
        ClosestStaticRayResultCallback( const btVector3& from, const btVector3&  to ) : btCollisionWorld::ClosestRayResultCallback( from, to ) {}

        virtual btScalar addSingleResult( btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace )
        {
                if ( !rayResult.m_collisionObject->isStaticObject() )
                        return 1.0;

                return ClosestRayResultCallback::addSingleResult( rayResult, normalInWorldSpace );
        }
};


bool nv::bullet_world::ray_test( const vec3& from, const vec3& to, vec3& hpos, vec3& hnorm, bool static_only )
{
        btVector3 bfrom( n2b( from ) );
        btVector3 bto( n2b( to ) );
        if ( !static_only )
        {
                btCollisionWorld::ClosestRayResultCallback raycb( bfrom, bto );
                SELF->rayTest( bfrom, bto, raycb );
                bool result = false;
                if ( raycb.hasHit() )
                {
                        result = true;
                        hpos = b2n( raycb.m_hitPointWorld );
                        hnorm = b2n( raycb.m_hitNormalWorld );
                }
                return result;
        }
        else
        {
                ClosestStaticRayResultCallback raycb( bfrom, bto );
                SELF->rayTest( bfrom, bto, raycb );
                bool result = false;
                if ( raycb.hasHit() )
                {
                        result = true;
                        hpos = b2n( raycb.m_hitPointWorld );
                        hnorm = b2n( raycb.m_hitNormalWorld );
                }
                return result;
        }
}

void bullet_world::release( collision_shape sh )
{
        if ( !sh.internal ) return;
        if ( sh.mesh );
        {
                btStridingMeshInterface* smi = static_cast<btStridingMeshInterface*>( sh.mesh );
                delete smi;
        }

        btCollisionShape* cs = static_cast<btCollisionShape*>( sh.internal );
        delete cs;

}

void bullet_world::release( constraint c )
{
        if ( !c.internal ) return;
        btTypedConstraint* tc = static_cast<btTypedConstraint*>( c.internal );
        SELF->removeConstraint( tc );
        delete tc;
}

void bullet_world::release( rigid_body body )
{
        if ( !body.internal ) return;
        btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
        SELF->removeRigidBody( rb );
        delete rb;
}