Skip to content

Commit

Permalink
object waking, query volume updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@cyrilgramblicka
cyrilgramblicka committed Sep 13, 2017
1 parent 0f7ee0c commit e96ddfd
Show file tree
Hide file tree
Showing 6 changed files with 160 additions and 134 deletions.
126 changes: 5 additions & 121 deletions src/otbullet/discrete_dynamics_world.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@

#include <LinearMath/btIDebugDraw.h>

#include <ot/glm/coal.h>

#include "ot_terrain_contact_common.h"

#include <comm/timer.h>
Expand Down Expand Up @@ -245,6 +243,8 @@ namespace ot {

res.setPersistentManifold(manifold);

uint tri_count = 0;

for (uints j = 0; j < _cow_internal.size(); j++) {

btCollisionObjectWrapper internal_obj_wrapper(_cow_internal[j]._parent,
Expand Down Expand Up @@ -330,6 +330,8 @@ namespace ot {
repair_tree_collision_pairs();
}

tri_count += _triangles.size();

if (_triangles.size() > 0) {

if (m_debugDrawer) {
Expand Down Expand Up @@ -368,7 +370,7 @@ namespace ot {

res.refreshContactPoints();

if (manifold->getNumContacts() == 0) {
if (manifold->getNumContacts() == 0 || tri_count == 0) {
getDispatcher()->releaseManifold(manifold);
_manifolds.get_item(rb->getTerrainManifoldHandle());
_manifolds.del(_manifolds.get_item(rb->getTerrainManifoldHandle()));
Expand Down Expand Up @@ -595,124 +597,6 @@ namespace ot {
return &tb->trees[tid];
}

void discrete_dynamics_world::query_volume_sphere(const double3& pos, float rad, coid::dynarray<btCollisionObject *>& result)
{
#ifdef _DEBUG
bt32BitAxisSweep3 * broad = dynamic_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
DASSERT(broad!=nullptr);
#else
bt32BitAxisSweep3 * broad = static_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
#endif

static coid::dynarray<const btDbvtNode *> _processing_stack(1024);
_processing_stack.reset();

const btDbvtBroadphase* raycast_acc = broad->getRaycastAccelerator();
DASSERT(raycast_acc);

const btDbvt * dyn_set = &raycast_acc->m_sets[0];
const btDbvt * stat_set = &raycast_acc->m_sets[1];

const btDbvtNode * cur_node = nullptr;

if (dyn_set && dyn_set->m_root) {
_processing_stack.push(dyn_set->m_root);
}

if (stat_set && stat_set->m_root) {
_processing_stack.push(stat_set->m_root);
}

while (_processing_stack.pop(cur_node)) {
const btVector3& bt_aabb_cen = cur_node->volume.Center();
const btVector3& bt_aabb_half = cur_node->volume.Extents();
glm::double3 aabb_cen(bt_aabb_cen[0], bt_aabb_cen[1], bt_aabb_cen[2]);
glm::double3 aabb_half(bt_aabb_half[0], bt_aabb_half[1], bt_aabb_half[2]);
if (coal::intersects_sphere_aabb(pos, (double)rad, aabb_cen, aabb_half, (double*)nullptr)) {
if (cur_node->isleaf()) {
if(cur_node->data){
btDbvtProxy* dat = reinterpret_cast<btDbvtProxy*>(cur_node->data);
result.push(reinterpret_cast<btCollisionObject*>(dat->m_clientObject));
}
}
else {
_processing_stack.push(cur_node->childs[0]);
_processing_stack.push(cur_node->childs[1]);
}
};
}
}

void discrete_dynamics_world::query_volume_frustum(const double3 & pos, const float4 * f_planes_norms, uint8 nplanes, bool include_partial,coid::dynarray<btCollisionObject*>& result)
{
#ifdef _DEBUG
bt32BitAxisSweep3 * broad = dynamic_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
DASSERT(broad != nullptr);
#else
bt32BitAxisSweep3 * broad = static_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
#endif
static coid::dynarray<const btDbvtNode *> _processing_stack(1024);
_processing_stack.reset();

const btDbvtBroadphase* raycast_acc = broad->getRaycastAccelerator();
DASSERT(raycast_acc);

const btDbvt * dyn_set = &raycast_acc->m_sets[0];
const btDbvt * stat_set = &raycast_acc->m_sets[1];

const btDbvtNode * cur_node = nullptr;

if (dyn_set && dyn_set->m_root) {
_processing_stack.push(dyn_set->m_root);
}

if (stat_set && stat_set->m_root) {
_processing_stack.push(stat_set->m_root);
}

btCollisionObject p_obj;

while (_processing_stack.pop(cur_node)) {
const btVector3& bt_aabb_cen = cur_node->volume.Center();
const btVector3& bt_aabb_half = cur_node->volume.Extents();
glm::double3 aabb_cen(bt_aabb_cen[0], bt_aabb_cen[1], bt_aabb_cen[2]);
glm::float3 aabb_half(bt_aabb_half[0], bt_aabb_half[1], bt_aabb_half[2]);

if (coal::intersects_frustum_aabb(aabb_cen, aabb_half, pos, f_planes_norms, nplanes, true)) {
if (cur_node->isleaf()) {
if(cur_node->data){
btDbvtProxy* dat = reinterpret_cast<btDbvtProxy*>(cur_node->data);
btCollisionObject* leaf_obj = reinterpret_cast<btCollisionObject*>(dat->m_clientObject);

const btVector3& cen = leaf_obj->getWorldTransform().getOrigin();
const float3 aabb_pos(float(cen[0] - pos.x), float(cen[1] - pos.y), float(cen[2] - pos.z));
bool passes = true;
for (uint8 p = 0; p < nplanes; p++) {
float3 n(f_planes_norms[p]);
btVector3 min, max;
btTransform t(btMatrix3x3(n.x, n.y, n.z, 0., 0., 0., 0., 0., 0.));
leaf_obj->getCollisionShape()->getAabb(t,min,max);
const float np = float(max[0] - min[0]) * 0.5f;
const float mp = glm::dot(n, aabb_pos) + f_planes_norms[p].w;
if ((include_partial ? mp + np : mp - np) < 0.0f) {
passes = false;
break;
}
}

if (passes) {
result.push(leaf_obj);
}
}
}
else {
_processing_stack.push(cur_node->childs[0]);
_processing_stack.push(cur_node->childs[1]);
}
};
}
}

void discrete_dynamics_world::debugDrawWorld()
{
if (!m_debugDrawer) {
Expand Down
125 changes: 122 additions & 3 deletions src/otbullet/discrete_dynamics_world.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include "physics_cfg.h"

#include <ot/glm/glm_types.h>
#include <ot/glm/coal.h>

#include <comm/dynarray.h>
#include <comm/hash/slothash.h>
Expand Down Expand Up @@ -181,9 +182,6 @@ class discrete_dynamics_world : public btDiscreteDynamicsWorld

virtual void removeRigidBody(btRigidBody* body) override;

void query_volume_sphere(const double3& pos, float rad, coid::dynarray<btCollisionObject *>& result);
void query_volume_frustum(const double3&pos, const float4 * f_planes_norms, uint8 nplanes, bool include_partial, coid::dynarray<btCollisionObject *>& result);

virtual void debugDrawWorld() override;

typedef bool (*fn_ext_collision)(
Expand Down Expand Up @@ -244,6 +242,127 @@ class discrete_dynamics_world : public btDiscreteDynamicsWorld

void get_obb(const btCollisionShape * cs, const btTransform& t, double3& cen, float3x3& basis);

template<class fn> // void (*fn)(btCollisionObject * obj)
void query_volume_sphere(const double3& pos, float rad, fn process_fn)
{
#ifdef _DEBUG
bt32BitAxisSweep3 * broad = dynamic_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
DASSERT(broad != nullptr);
#else
bt32BitAxisSweep3 * broad = static_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
#endif

static coid::dynarray<const btDbvtNode *> _processing_stack(1024);
_processing_stack.reset();

const btDbvtBroadphase* raycast_acc = broad->getRaycastAccelerator();
DASSERT(raycast_acc);

const btDbvt * dyn_set = &raycast_acc->m_sets[0];
const btDbvt * stat_set = &raycast_acc->m_sets[1];

const btDbvtNode * cur_node = nullptr;

if (dyn_set && dyn_set->m_root) {
_processing_stack.push(dyn_set->m_root);
}

if (stat_set && stat_set->m_root) {
_processing_stack.push(stat_set->m_root);
}

while (_processing_stack.pop(cur_node)) {
const btVector3& bt_aabb_cen = cur_node->volume.Center();
const btVector3& bt_aabb_half = cur_node->volume.Extents();
double3 aabb_cen(bt_aabb_cen[0], bt_aabb_cen[1], bt_aabb_cen[2]);
double3 aabb_half(bt_aabb_half[0], bt_aabb_half[1], bt_aabb_half[2]);
if (coal::intersects_sphere_aabb(pos, (double)rad, aabb_cen, aabb_half, (double*)nullptr)) {
if (cur_node->isleaf()) {
if (cur_node->data) {
btDbvtProxy* dat = reinterpret_cast<btDbvtProxy*>(cur_node->data);
process_fn(reinterpret_cast<btCollisionObject*>(dat->m_clientObject));
}
}
else {
_processing_stack.push(cur_node->childs[0]);
_processing_stack.push(cur_node->childs[1]);
}
}
}
}

template<class fn> // void (*fn)(btCollisionObject * obj)
void query_volume_frustum(const double3&pos, const float4 * f_planes_norms, uint8 nplanes, bool include_partial, fn process_fn)
{
#ifdef _DEBUG
bt32BitAxisSweep3 * broad = dynamic_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
DASSERT(broad != nullptr);
#else
bt32BitAxisSweep3 * broad = static_cast<bt32BitAxisSweep3 *>(m_broadphasePairCache);
#endif
static coid::dynarray<const btDbvtNode *> _processing_stack(1024);
_processing_stack.reset();

const btDbvtBroadphase* raycast_acc = broad->getRaycastAccelerator();
DASSERT(raycast_acc);

const btDbvt * dyn_set = &raycast_acc->m_sets[0];
const btDbvt * stat_set = &raycast_acc->m_sets[1];

const btDbvtNode * cur_node = nullptr;

if (dyn_set && dyn_set->m_root) {
_processing_stack.push(dyn_set->m_root);
}

if (stat_set && stat_set->m_root) {
_processing_stack.push(stat_set->m_root);
}

btCollisionObject p_obj;

while (_processing_stack.pop(cur_node)) {
const btVector3& bt_aabb_cen = cur_node->volume.Center();
const btVector3& bt_aabb_half = cur_node->volume.Extents();
double3 aabb_cen(bt_aabb_cen[0], bt_aabb_cen[1], bt_aabb_cen[2]);
float3 aabb_half(bt_aabb_half[0], bt_aabb_half[1], bt_aabb_half[2]);

if (coal::intersects_frustum_aabb(aabb_cen, aabb_half, pos, f_planes_norms, nplanes, true)) {
if (cur_node->isleaf()) {
if (cur_node->data) {
btDbvtProxy* dat = reinterpret_cast<btDbvtProxy*>(cur_node->data);
btCollisionObject* leaf_obj = reinterpret_cast<btCollisionObject*>(dat->m_clientObject);

const btVector3& cen = leaf_obj->getWorldTransform().getOrigin();
const float3 aabb_pos(float(cen[0] - pos.x), float(cen[1] - pos.y), float(cen[2] - pos.z));
bool passes = true;
for (uint8 p = 0; p < nplanes; p++) {
float3 n(f_planes_norms[p]);
btVector3 min, max;
btTransform t(btMatrix3x3(n.x, n.y, n.z, 0., 0., 0., 0., 0., 0.));
leaf_obj->getCollisionShape()->getAabb(t, min, max);
const float np = float(max[0] - min[0]) * 0.5f;
const float mp = glm::dot(n, aabb_pos) + f_planes_norms[p].w;
if ((include_partial ? mp + np : mp - np) < 0.0f) {
passes = false;
break;
}
}

if (passes) {
process_fn(leaf_obj);
}
}
}
else {
_processing_stack.push(cur_node->childs[0]);
_processing_stack.push(cur_node->childs[1]);
}
};
}
}



protected:

Expand Down
1 change: 1 addition & 0 deletions src/otbullet/otbullet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ class physics : public policy_intrusive_base

ifc_fn void query_volume_sphere(const double3 & pos, float rad, ifc_inout coid::dynarray<btCollisionObject*>& result);
ifc_fn void query_volume_frustum(const double3 & pos, const float4 * f_planes_norms, uint8 nplanes, bool include_partial, ifc_inout coid::dynarray<btCollisionObject *>& result);
ifc_fn void wake_up_objects_in_radius(const double3 & pos, float rad);


ifc_event bool terrain_collisions(
Expand Down
10 changes: 6 additions & 4 deletions src/otbullet/otbullet.intergen.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class physics_dispatcher : public physics
{
if (_vtable1) return _vtable1;

_vtable1 = new ifn_t[34];
_vtable1 = new ifn_t[35];
_vtable1[0] = reinterpret_cast<ifn_t>(static_cast<void(policy_intrusive_base::*)(double)>(&::physics::step_simulation));
_vtable1[1] = reinterpret_cast<ifn_t>(static_cast<void(policy_intrusive_base::*)(const double[3],const double[3],void*)>(&::physics::ray_test));
_vtable1[2] = reinterpret_cast<ifn_t>(static_cast<btRigidBody*(policy_intrusive_base::*)()>(&::physics::fixed_object));
Expand Down Expand Up @@ -72,6 +72,7 @@ class physics_dispatcher : public physics
_vtable1[31] = reinterpret_cast<ifn_t>(static_cast<void(policy_intrusive_base::*)()>(&::physics::debug_draw_world));
_vtable1[32] = reinterpret_cast<ifn_t>(static_cast<void(policy_intrusive_base::*)(const double3&,float,coid::dynarray<btCollisionObject*>&)>(&::physics::query_volume_sphere));
_vtable1[33] = reinterpret_cast<ifn_t>(static_cast<void(policy_intrusive_base::*)(const double3&,const float4*,uint8,bool,coid::dynarray<btCollisionObject *>&)>(&::physics::query_volume_frustum));
_vtable1[34] = reinterpret_cast<ifn_t>(static_cast<void(policy_intrusive_base::*)(const double3&,float)>(&::physics::wake_up_objects_in_radius));
return _vtable1;
}

Expand All @@ -83,7 +84,7 @@ class physics_dispatcher : public physics
if (_vtable2) return _vtable2;
ifn_t* vtable1 = get_vtable();

_vtable2 = new ifn_t[34];
_vtable2 = new ifn_t[35];
_vtable2[0] = vtable1[0];
_vtable2[1] = vtable1[1];
_vtable2[2] = vtable1[2];
Expand Down Expand Up @@ -118,6 +119,7 @@ class physics_dispatcher : public physics
_vtable2[31] = vtable1[31];
_vtable2[32] = vtable1[32];
_vtable2[33] = vtable1[33];
_vtable2[34] = vtable1[34];
return _vtable2;
}

Expand Down Expand Up @@ -198,10 +200,10 @@ class physics_dispatcher : public physics
on ? (void*)&_generic_interface_creator : nullptr);

interface_register::register_interface_creator(
"bt::physics.create@3573161280",
"bt::physics.create@2719886067",
on ? (void*)&create : nullptr);
interface_register::register_interface_creator(
"bt::physics.get@3573161280",
"bt::physics.get@2719886067",
on ? (void*)&get : nullptr);
}
};
Expand Down
Loading

0 comments on commit e96ddfd

Please sign in to comment.