Merge pull request #12641 from AndreaCatania/bullet
Bullet physics wrapper
This commit is contained in:
commit
a89fa34c21
191
modules/bullet/SCsub
Normal file
191
modules/bullet/SCsub
Normal file
|
@ -0,0 +1,191 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
Import('env')
|
||||
|
||||
# build only version 2
|
||||
# Bullet 2.87
|
||||
bullet_src__2_x = [
|
||||
# BulletCollision
|
||||
"BulletCollision/BroadphaseCollision/btAxisSweep3.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btDbvt.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btDispatcher.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp"
|
||||
, "BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCollisionObject.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCollisionWorld.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btGhostObject.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btManifoldResult.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/CollisionDispatch/btUnionFind.cpp"
|
||||
, "BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp"
|
||||
, "BulletCollision/CollisionShapes/btBoxShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btBox2dShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btCapsuleShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btCollisionShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btCompoundShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConcaveShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConeShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvexHullShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvexInternalShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvexPolyhedron.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvexShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvex2dShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btCylinderShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btEmptyShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btMultiSphereShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btOptimizedBvh.cpp"
|
||||
, "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btShapeHull.cpp"
|
||||
, "BulletCollision/CollisionShapes/btSphereShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btStridingMeshInterface.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTetrahedronShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTriangleBuffer.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTriangleCallback.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTriangleMesh.cpp"
|
||||
, "BulletCollision/CollisionShapes/btTriangleMeshShape.cpp"
|
||||
, "BulletCollision/CollisionShapes/btUniformScalingShape.cpp"
|
||||
, "BulletCollision/Gimpact/btContactProcessing.cpp"
|
||||
, "BulletCollision/Gimpact/btGenericPoolAllocator.cpp"
|
||||
, "BulletCollision/Gimpact/btGImpactBvh.cpp"
|
||||
, "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp"
|
||||
, "BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp"
|
||||
, "BulletCollision/Gimpact/btGImpactShape.cpp"
|
||||
, "BulletCollision/Gimpact/btTriangleShapeEx.cpp"
|
||||
, "BulletCollision/Gimpact/gim_box_set.cpp"
|
||||
, "BulletCollision/Gimpact/gim_contact.cpp"
|
||||
, "BulletCollision/Gimpact/gim_memory.cpp"
|
||||
, "BulletCollision/Gimpact/gim_tri_collision.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btConvexCast.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp"
|
||||
, "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp"
|
||||
|
||||
# BulletDynamics
|
||||
, "BulletDynamics/Character/btKinematicCharacterController.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btContactConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btFixedConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btGearConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btSliderConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btTypedConstraint.cpp"
|
||||
, "BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp"
|
||||
, "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp"
|
||||
, "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp"
|
||||
, "BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp"
|
||||
, "BulletDynamics/Dynamics/btRigidBody.cpp"
|
||||
, "BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp"
|
||||
#, "BulletDynamics/Dynamics/Bullet-C-API.cpp"
|
||||
, "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
|
||||
, "BulletDynamics/Vehicle/btWheelInfo.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBody.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"
|
||||
, "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"
|
||||
, "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp"
|
||||
, "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp"
|
||||
, "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp"
|
||||
|
||||
# BulletInverseDynamics
|
||||
, "BulletInverseDynamics/IDMath.cpp"
|
||||
, "BulletInverseDynamics/MultiBodyTree.cpp"
|
||||
, "BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp"
|
||||
, "BulletInverseDynamics/details/MultiBodyTreeImpl.cpp"
|
||||
|
||||
# BulletSoftBody
|
||||
, "BulletSoftBody/btSoftBody.cpp"
|
||||
, "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp"
|
||||
, "BulletSoftBody/btSoftBodyHelpers.cpp"
|
||||
, "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp"
|
||||
, "BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp"
|
||||
, "BulletSoftBody/btSoftRigidDynamicsWorld.cpp"
|
||||
, "BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp"
|
||||
, "BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp"
|
||||
, "BulletSoftBody/btDefaultSoftBodySolver.cpp"
|
||||
|
||||
# clew
|
||||
, "clew/clew.c"
|
||||
|
||||
# LinearMath
|
||||
, "LinearMath/btAlignedAllocator.cpp"
|
||||
, "LinearMath/btConvexHull.cpp"
|
||||
, "LinearMath/btConvexHullComputer.cpp"
|
||||
, "LinearMath/btGeometryUtil.cpp"
|
||||
, "LinearMath/btPolarDecomposition.cpp"
|
||||
, "LinearMath/btQuickprof.cpp"
|
||||
, "LinearMath/btSerializer.cpp"
|
||||
, "LinearMath/btSerializer64.cpp"
|
||||
, "LinearMath/btThreads.cpp"
|
||||
, "LinearMath/btVector3.cpp"
|
||||
]
|
||||
|
||||
thirdparty_dir = "#thirdparty/bullet/"
|
||||
thirdparty_src = thirdparty_dir + "src/"
|
||||
|
||||
bullet_sources = [thirdparty_src + file for file in bullet_src__2_x]
|
||||
|
||||
# include headers
|
||||
env.Append(CPPPATH=[thirdparty_src])
|
||||
|
||||
env.add_source_files(env.modules_sources, bullet_sources)
|
||||
|
||||
# Godot source files
|
||||
env.add_source_files(env.modules_sources, "*.cpp")
|
||||
|
||||
Export('env')
|
33
modules/bullet/SCsub_with_lib
Normal file
33
modules/bullet/SCsub_with_lib
Normal file
|
@ -0,0 +1,33 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
Import('env')
|
||||
|
||||
thirdparty_dir = "#thirdparty/bullet/"
|
||||
thirdparty_lib = thirdparty_dir + "Win64/lib/"
|
||||
|
||||
bullet_libs = [
|
||||
"Bullet2FileLoader",
|
||||
"Bullet3Collision",
|
||||
"Bullet3Common",
|
||||
"Bullet3Dynamics",
|
||||
"Bullet3Geometry",
|
||||
"Bullet3OpenCL_clew",
|
||||
"BulletCollision",
|
||||
"BulletDynamics",
|
||||
"BulletInverseDynamics",
|
||||
"BulletSoftBody",
|
||||
"LinearMath"
|
||||
]
|
||||
|
||||
thirdparty_src = thirdparty_dir + "src/"
|
||||
# include headers
|
||||
env.Append(CPPPATH=[thirdparty_src])
|
||||
|
||||
# lib
|
||||
env.Append(LIBPATH=[thirdparty_dir + "/Win64/lib/"])
|
||||
|
||||
bullet_libs = [file+'.lib' for file in bullet_libs]
|
||||
# LIBS doesn't work in windows
|
||||
env.Append(LINKFLAGS=bullet_libs)
|
||||
|
||||
env.add_source_files(env.modules_sources, "*.cpp")
|
284
modules/bullet/area_bullet.cpp
Normal file
284
modules/bullet/area_bullet.cpp
Normal file
|
@ -0,0 +1,284 @@
|
|||
/*************************************************************************/
|
||||
/* area_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "area_bullet.h"
|
||||
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
|
||||
#include "btBulletCollisionCommon.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "collision_object_bullet.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
AreaBullet::AreaBullet()
|
||||
: RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_AREA),
|
||||
monitorable(true),
|
||||
isScratched(false),
|
||||
spOv_mode(PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED),
|
||||
spOv_gravityPoint(false),
|
||||
spOv_gravityPointDistanceScale(0),
|
||||
spOv_gravityPointAttenuation(1),
|
||||
spOv_gravityVec(0, -1, 0),
|
||||
spOv_gravityMag(10),
|
||||
spOv_linearDump(0.1),
|
||||
spOv_angularDump(1),
|
||||
spOv_priority(0) {
|
||||
|
||||
btGhost = bulletnew(btGhostObject);
|
||||
btGhost->setCollisionShape(compoundShape);
|
||||
setupBulletCollisionObject(btGhost);
|
||||
/// Collision objects with a callback still have collision response with dynamic rigid bodies.
|
||||
/// In order to use collision objects as trigger, you have to disable the collision response.
|
||||
set_collision_enabled(false);
|
||||
|
||||
for (int i = 0; i < 5; ++i)
|
||||
call_event_res_ptr[i] = &call_event_res[i];
|
||||
}
|
||||
|
||||
AreaBullet::~AreaBullet() {
|
||||
remove_all_overlapping_instantly();
|
||||
}
|
||||
|
||||
void AreaBullet::dispatch_callbacks() {
|
||||
if (!isScratched)
|
||||
return;
|
||||
isScratched = false;
|
||||
|
||||
// Reverse order because I've to remove EXIT objects
|
||||
for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
|
||||
OverlappingObjectData &otherObj = overlappingObjects[i];
|
||||
|
||||
switch (otherObj.state) {
|
||||
case OVERLAP_STATE_ENTER:
|
||||
otherObj.state = OVERLAP_STATE_INSIDE;
|
||||
call_event(otherObj.object, PhysicsServer::AREA_BODY_ADDED);
|
||||
otherObj.object->on_enter_area(this);
|
||||
break;
|
||||
case OVERLAP_STATE_EXIT:
|
||||
call_event(otherObj.object, PhysicsServer::AREA_BODY_REMOVED);
|
||||
otherObj.object->on_exit_area(this);
|
||||
overlappingObjects.remove(i); // Remove after callback
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AreaBullet::call_event(CollisionObjectBullet *p_otherObject, PhysicsServer::AreaBodyStatus p_status) {
|
||||
|
||||
InOutEventCallback &event = eventsCallbacks[static_cast<int>(p_otherObject->getType())];
|
||||
Object *areaGodoObject = ObjectDB::get_instance(event.event_callback_id);
|
||||
|
||||
if (!areaGodoObject) {
|
||||
event.event_callback_id = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
call_event_res[0] = p_status;
|
||||
call_event_res[1] = p_otherObject->get_self(); // Other body
|
||||
call_event_res[2] = p_otherObject->get_instance_id(); // instance ID
|
||||
call_event_res[3] = 0; // other_body_shape ID
|
||||
call_event_res[4] = 0; // self_shape ID
|
||||
|
||||
Variant::CallError outResp;
|
||||
areaGodoObject->call(event.event_callback_method, (const Variant **)call_event_res_ptr, 5, outResp);
|
||||
}
|
||||
|
||||
void AreaBullet::scratch() {
|
||||
if (isScratched)
|
||||
return;
|
||||
isScratched = true;
|
||||
}
|
||||
|
||||
void AreaBullet::remove_all_overlapping_instantly() {
|
||||
CollisionObjectBullet *supportObject;
|
||||
for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
|
||||
supportObject = overlappingObjects[i].object;
|
||||
call_event(supportObject, PhysicsServer::AREA_BODY_REMOVED);
|
||||
supportObject->on_exit_area(this);
|
||||
}
|
||||
overlappingObjects.clear();
|
||||
}
|
||||
|
||||
void AreaBullet::remove_overlapping_instantly(CollisionObjectBullet *p_object) {
|
||||
CollisionObjectBullet *supportObject;
|
||||
for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
|
||||
supportObject = overlappingObjects[i].object;
|
||||
if (supportObject == p_object) {
|
||||
call_event(supportObject, PhysicsServer::AREA_BODY_REMOVED);
|
||||
supportObject->on_exit_area(this);
|
||||
overlappingObjects.remove(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int AreaBullet::find_overlapping_object(CollisionObjectBullet *p_colObj) {
|
||||
const int size = overlappingObjects.size();
|
||||
for (int i = 0; i < size; ++i) {
|
||||
if (overlappingObjects[i].object == p_colObj) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void AreaBullet::set_monitorable(bool p_monitorable) {
|
||||
monitorable = p_monitorable;
|
||||
}
|
||||
|
||||
bool AreaBullet::is_monitoring() const {
|
||||
return get_godot_object_flags() & GOF_IS_MONITORING_AREA;
|
||||
}
|
||||
|
||||
void AreaBullet::reload_body() {
|
||||
if (space) {
|
||||
space->remove_area(this);
|
||||
space->add_area(this);
|
||||
}
|
||||
}
|
||||
|
||||
void AreaBullet::set_space(SpaceBullet *p_space) {
|
||||
// Clear the old space if there is one
|
||||
if (space) {
|
||||
isScratched = false;
|
||||
|
||||
// Remove this object form the physics world
|
||||
space->remove_area(this);
|
||||
}
|
||||
|
||||
space = p_space;
|
||||
|
||||
if (space) {
|
||||
space->add_area(this);
|
||||
}
|
||||
}
|
||||
|
||||
void AreaBullet::on_collision_filters_change() {
|
||||
if (space) {
|
||||
space->reload_collision_filters(this);
|
||||
}
|
||||
}
|
||||
|
||||
void AreaBullet::add_overlap(CollisionObjectBullet *p_otherObject) {
|
||||
scratch();
|
||||
overlappingObjects.push_back(OverlappingObjectData(p_otherObject, OVERLAP_STATE_ENTER));
|
||||
p_otherObject->notify_new_overlap(this);
|
||||
}
|
||||
|
||||
void AreaBullet::put_overlap_as_exit(int p_index) {
|
||||
scratch();
|
||||
overlappingObjects[p_index].state = OVERLAP_STATE_EXIT;
|
||||
}
|
||||
|
||||
void AreaBullet::put_overlap_as_inside(int p_index) {
|
||||
// This check is required to be sure this body was inside
|
||||
if (OVERLAP_STATE_DIRTY == overlappingObjects[p_index].state) {
|
||||
overlappingObjects[p_index].state = OVERLAP_STATE_INSIDE;
|
||||
}
|
||||
}
|
||||
|
||||
void AreaBullet::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY:
|
||||
set_spOv_gravityMag(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
|
||||
set_spOv_gravityVec(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
|
||||
set_spOv_linearDump(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
|
||||
set_spOv_angularDump(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_PRIORITY:
|
||||
set_spOv_priority(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
|
||||
set_spOv_gravityPoint(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
|
||||
set_spOv_gravityPointDistanceScale(p_value);
|
||||
break;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
|
||||
set_spOv_gravityPointAttenuation(p_value);
|
||||
break;
|
||||
default:
|
||||
print_line("The Bullet areas dosn't suppot this param: " + itos(p_param));
|
||||
}
|
||||
}
|
||||
|
||||
Variant AreaBullet::get_param(PhysicsServer::AreaParameter p_param) const {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY:
|
||||
return spOv_gravityMag;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
|
||||
return spOv_gravityVec;
|
||||
case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
|
||||
return spOv_linearDump;
|
||||
case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
|
||||
return spOv_angularDump;
|
||||
case PhysicsServer::AREA_PARAM_PRIORITY:
|
||||
return spOv_priority;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
|
||||
return spOv_gravityPoint;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
|
||||
return spOv_gravityPointDistanceScale;
|
||||
case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
|
||||
return spOv_gravityPointAttenuation;
|
||||
default:
|
||||
print_line("The Bullet areas dosn't suppot this param: " + itos(p_param));
|
||||
return Variant();
|
||||
}
|
||||
}
|
||||
|
||||
void AreaBullet::set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method) {
|
||||
InOutEventCallback &ev = eventsCallbacks[static_cast<int>(p_callbackObjectType)];
|
||||
ev.event_callback_id = p_id;
|
||||
ev.event_callback_method = p_method;
|
||||
|
||||
/// Set if monitoring
|
||||
if (eventsCallbacks[0].event_callback_id || eventsCallbacks[1].event_callback_id) {
|
||||
set_godot_object_flags(get_godot_object_flags() | GOF_IS_MONITORING_AREA);
|
||||
} else {
|
||||
set_godot_object_flags(get_godot_object_flags() & (~GOF_IS_MONITORING_AREA));
|
||||
}
|
||||
}
|
||||
|
||||
bool AreaBullet::has_event_callback(Type p_callbackObjectType) {
|
||||
return eventsCallbacks[static_cast<int>(p_callbackObjectType)].event_callback_id;
|
||||
}
|
||||
|
||||
void AreaBullet::on_enter_area(AreaBullet *p_area) {
|
||||
}
|
||||
|
||||
void AreaBullet::on_exit_area(AreaBullet *p_area) {
|
||||
CollisionObjectBullet::on_exit_area(p_area);
|
||||
}
|
169
modules/bullet/area_bullet.h
Normal file
169
modules/bullet/area_bullet.h
Normal file
|
@ -0,0 +1,169 @@
|
|||
/*************************************************************************/
|
||||
/* area_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef AREABULLET_H
|
||||
#define AREABULLET_H
|
||||
|
||||
#include "collision_object_bullet.h"
|
||||
#include "core/vector.h"
|
||||
#include "servers/physics_server.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
class btGhostObject;
|
||||
|
||||
class AreaBullet : public RigidCollisionObjectBullet {
|
||||
friend void SpaceBullet::check_ghost_overlaps();
|
||||
|
||||
public:
|
||||
struct InOutEventCallback {
|
||||
ObjectID event_callback_id;
|
||||
StringName event_callback_method;
|
||||
|
||||
InOutEventCallback()
|
||||
: event_callback_id(0) {}
|
||||
};
|
||||
|
||||
enum OverlapState {
|
||||
OVERLAP_STATE_DIRTY = 0, // Mark processed overlaps
|
||||
OVERLAP_STATE_INSIDE, // Mark old overlap
|
||||
OVERLAP_STATE_ENTER, // Mark just enter overlap
|
||||
OVERLAP_STATE_EXIT // Mark ended overlaps
|
||||
};
|
||||
|
||||
struct OverlappingObjectData {
|
||||
CollisionObjectBullet *object;
|
||||
OverlapState state;
|
||||
|
||||
OverlappingObjectData()
|
||||
: object(NULL), state(OVERLAP_STATE_ENTER) {}
|
||||
OverlappingObjectData(CollisionObjectBullet *p_object, OverlapState p_state)
|
||||
: object(p_object), state(p_state) {}
|
||||
OverlappingObjectData(const OverlappingObjectData &other) {
|
||||
operator=(other);
|
||||
}
|
||||
void operator=(const OverlappingObjectData &other) {
|
||||
object = other.object;
|
||||
state = other.state;
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
// These are used by function callEvent. Instead to create this each call I create if one time.
|
||||
Variant call_event_res[5];
|
||||
Variant *call_event_res_ptr[5];
|
||||
|
||||
btGhostObject *btGhost;
|
||||
Vector<OverlappingObjectData> overlappingObjects;
|
||||
bool monitorable;
|
||||
|
||||
PhysicsServer::AreaSpaceOverrideMode spOv_mode;
|
||||
bool spOv_gravityPoint;
|
||||
real_t spOv_gravityPointDistanceScale;
|
||||
real_t spOv_gravityPointAttenuation;
|
||||
Vector3 spOv_gravityVec;
|
||||
real_t spOv_gravityMag;
|
||||
real_t spOv_linearDump;
|
||||
real_t spOv_angularDump;
|
||||
int spOv_priority;
|
||||
|
||||
bool isScratched;
|
||||
|
||||
InOutEventCallback eventsCallbacks[2];
|
||||
|
||||
public:
|
||||
AreaBullet();
|
||||
~AreaBullet();
|
||||
|
||||
_FORCE_INLINE_ btGhostObject *get_bt_ghost() const { return btGhost; }
|
||||
int find_overlapping_object(CollisionObjectBullet *p_colObj);
|
||||
|
||||
void set_monitorable(bool p_monitorable);
|
||||
_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
|
||||
|
||||
bool is_monitoring() const;
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) { spOv_mode = p_mode; }
|
||||
_FORCE_INLINE_ PhysicsServer::AreaSpaceOverrideMode get_spOv_mode() { return spOv_mode; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_gravityPoint(bool p_isGP) { spOv_gravityPoint = p_isGP; }
|
||||
_FORCE_INLINE_ bool is_spOv_gravityPoint() { return spOv_gravityPoint; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_gravityPointDistanceScale(real_t p_GPDS) { spOv_gravityPointDistanceScale = p_GPDS; }
|
||||
_FORCE_INLINE_ real_t get_spOv_gravityPointDistanceScale() { return spOv_gravityPointDistanceScale; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_gravityPointAttenuation(real_t p_GPA) { spOv_gravityPointAttenuation = p_GPA; }
|
||||
_FORCE_INLINE_ real_t get_spOv_gravityPointAttenuation() { return spOv_gravityPointAttenuation; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_gravityVec(Vector3 p_vec) { spOv_gravityVec = p_vec; }
|
||||
_FORCE_INLINE_ const Vector3 &get_spOv_gravityVec() const { return spOv_gravityVec; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_gravityMag(real_t p_gravityMag) { spOv_gravityMag = p_gravityMag; }
|
||||
_FORCE_INLINE_ real_t get_spOv_gravityMag() { return spOv_gravityMag; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_linearDump(real_t p_linearDump) { spOv_linearDump = p_linearDump; }
|
||||
_FORCE_INLINE_ real_t get_spOv_linearDamp() { return spOv_linearDump; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_angularDump(real_t p_angularDump) { spOv_angularDump = p_angularDump; }
|
||||
_FORCE_INLINE_ real_t get_spOv_angularDamp() { return spOv_angularDump; }
|
||||
|
||||
_FORCE_INLINE_ void set_spOv_priority(int p_priority) { spOv_priority = p_priority; }
|
||||
_FORCE_INLINE_ int get_spOv_priority() { return spOv_priority; }
|
||||
|
||||
virtual void reload_body();
|
||||
virtual void set_space(SpaceBullet *p_space);
|
||||
|
||||
virtual void dispatch_callbacks();
|
||||
void call_event(CollisionObjectBullet *p_otherObject, PhysicsServer::AreaBodyStatus p_status);
|
||||
void set_on_state_change(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
|
||||
void scratch();
|
||||
|
||||
void remove_all_overlapping_instantly();
|
||||
// Dispatch the callbacks and removes from overlapping list
|
||||
void remove_overlapping_instantly(CollisionObjectBullet *p_object);
|
||||
|
||||
virtual void on_collision_filters_change();
|
||||
virtual void on_collision_checker_start() {}
|
||||
|
||||
void add_overlap(CollisionObjectBullet *p_otherObject);
|
||||
void put_overlap_as_exit(int p_index);
|
||||
void put_overlap_as_inside(int p_index);
|
||||
|
||||
void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value);
|
||||
Variant get_param(PhysicsServer::AreaParameter p_param) const;
|
||||
|
||||
void set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method);
|
||||
bool has_event_callback(Type p_callbackObjectType);
|
||||
|
||||
virtual void on_enter_area(AreaBullet *p_area);
|
||||
virtual void on_exit_area(AreaBullet *p_area);
|
||||
};
|
||||
|
||||
#endif
|
94
modules/bullet/btRayShape.cpp
Normal file
94
modules/bullet/btRayShape.cpp
Normal file
|
@ -0,0 +1,94 @@
|
|||
/*************************************************************************/
|
||||
/* btRayShape.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "btRayShape.h"
|
||||
#include "LinearMath/btAabbUtil2.h"
|
||||
#include "math/math_funcs.h"
|
||||
|
||||
btRayShape::btRayShape(btScalar length)
|
||||
: btConvexInternalShape(),
|
||||
m_shapeAxis(0, 0, 1) {
|
||||
m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE;
|
||||
setLength(length);
|
||||
}
|
||||
|
||||
btRayShape::~btRayShape() {
|
||||
}
|
||||
|
||||
void btRayShape::setLength(btScalar p_length) {
|
||||
|
||||
m_length = p_length;
|
||||
reload_cache();
|
||||
}
|
||||
|
||||
btVector3 btRayShape::localGetSupportingVertex(const btVector3 &vec) const {
|
||||
return localGetSupportingVertexWithoutMargin(vec) + (m_shapeAxis * m_collisionMargin);
|
||||
}
|
||||
|
||||
btVector3 btRayShape::localGetSupportingVertexWithoutMargin(const btVector3 &vec) const {
|
||||
if (vec.z() > 0)
|
||||
return m_shapeAxis * m_cacheScaledLength;
|
||||
else
|
||||
return btVector3(0, 0, 0);
|
||||
}
|
||||
|
||||
void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const {
|
||||
for (int i = 0; i < numVectors; ++i) {
|
||||
supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
|
||||
#define MARGIN_BROADPHASE 0.1
|
||||
btVector3 localAabbMin(0, 0, 0);
|
||||
btVector3 localAabbMax(m_shapeAxis * m_length);
|
||||
btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax);
|
||||
}
|
||||
|
||||
void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
|
||||
inertia.setZero();
|
||||
}
|
||||
|
||||
int btRayShape::getNumPreferredPenetrationDirections() const {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void btRayShape::getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const {
|
||||
penetrationVector.setZero();
|
||||
}
|
||||
|
||||
void btRayShape::reload_cache() {
|
||||
|
||||
m_cacheScaledLength = m_length * m_localScaling[2] + m_collisionMargin;
|
||||
|
||||
m_cacheSupportPoint.setIdentity();
|
||||
m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength);
|
||||
}
|
87
modules/bullet/btRayShape.h
Normal file
87
modules/bullet/btRayShape.h
Normal file
|
@ -0,0 +1,87 @@
|
|||
/*************************************************************************/
|
||||
/* btRayShape.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
/// IMPORTANT The class name and filename was created by following Bullet writing rules for an easy (eventually ) porting to bullet
|
||||
/// This shape is a custom shape that is not present to Bullet physics engine
|
||||
#ifndef BTRAYSHAPE_H
|
||||
#define BTRAYSHAPE_H
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
||||
|
||||
/// Ray shape around z axis
|
||||
ATTRIBUTE_ALIGNED16(class)
|
||||
btRayShape : public btConvexInternalShape {
|
||||
|
||||
btScalar m_length;
|
||||
/// The default axis is the z
|
||||
btVector3 m_shapeAxis;
|
||||
|
||||
btTransform m_cacheSupportPoint;
|
||||
btScalar m_cacheScaledLength;
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btRayShape(btScalar length);
|
||||
virtual ~btRayShape();
|
||||
|
||||
void setLength(btScalar p_length);
|
||||
btScalar getLength() const { return m_length; }
|
||||
|
||||
const btTransform &getSupportPoint() const { return m_cacheSupportPoint; }
|
||||
const btScalar &getScaledLength() const { return m_cacheScaledLength; }
|
||||
|
||||
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const;
|
||||
#ifndef __SPU__
|
||||
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const;
|
||||
#endif //#ifndef __SPU__
|
||||
|
||||
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const;
|
||||
|
||||
///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
|
||||
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const;
|
||||
|
||||
#ifndef __SPU__
|
||||
virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
|
||||
|
||||
virtual const char *getName() const {
|
||||
return "RayZ";
|
||||
}
|
||||
#endif //__SPU__
|
||||
|
||||
virtual int getNumPreferredPenetrationDirections() const;
|
||||
virtual void getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const;
|
||||
|
||||
private:
|
||||
void reload_cache();
|
||||
};
|
||||
|
||||
#endif // BTRAYSHAPE_H
|
1339
modules/bullet/bullet_physics_server.cpp
Normal file
1339
modules/bullet/bullet_physics_server.cpp
Normal file
File diff suppressed because it is too large
Load diff
358
modules/bullet/bullet_physics_server.h
Normal file
358
modules/bullet/bullet_physics_server.h
Normal file
|
@ -0,0 +1,358 @@
|
|||
/*************************************************************************/
|
||||
/* bullet_physics_server.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BULLET_PHYSICS_SERVER_H
|
||||
#define BULLET_PHYSICS_SERVER_H
|
||||
|
||||
#include "area_bullet.h"
|
||||
#include "joint_bullet.h"
|
||||
#include "rid.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
#include "servers/physics_server.h"
|
||||
#include "shape_bullet.h"
|
||||
#include "soft_body_bullet.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
class BulletPhysicsServer : public PhysicsServer {
|
||||
GDCLASS(BulletPhysicsServer, PhysicsServer)
|
||||
|
||||
friend class BulletPhysicsDirectSpaceState;
|
||||
|
||||
bool active;
|
||||
SpaceBullet *activeSpace;
|
||||
|
||||
mutable RID_Owner<SpaceBullet> space_owner;
|
||||
mutable RID_Owner<ShapeBullet> shape_owner;
|
||||
mutable RID_Owner<AreaBullet> area_owner;
|
||||
mutable RID_Owner<RigidBodyBullet> rigid_body_owner;
|
||||
mutable RID_Owner<SoftBodyBullet> soft_body_owner;
|
||||
mutable RID_Owner<JointBullet> joint_owner;
|
||||
|
||||
private:
|
||||
/// This is used when a collision shape is not active, so the bullet compound shapes index are always sync with godot index
|
||||
static btEmptyShape *emptyShape;
|
||||
|
||||
public:
|
||||
static btEmptyShape *get_empty_shape();
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
public:
|
||||
BulletPhysicsServer();
|
||||
~BulletPhysicsServer();
|
||||
|
||||
_FORCE_INLINE_ RID_Owner<SpaceBullet> *get_space_owner() {
|
||||
return &space_owner;
|
||||
}
|
||||
_FORCE_INLINE_ RID_Owner<ShapeBullet> *get_shape_owner() {
|
||||
return &shape_owner;
|
||||
}
|
||||
_FORCE_INLINE_ RID_Owner<AreaBullet> *get_area_owner() {
|
||||
return &area_owner;
|
||||
}
|
||||
_FORCE_INLINE_ RID_Owner<RigidBodyBullet> *get_rigid_body_owner() {
|
||||
return &rigid_body_owner;
|
||||
}
|
||||
_FORCE_INLINE_ RID_Owner<SoftBodyBullet> *get_soft_body_owner() {
|
||||
return &soft_body_owner;
|
||||
}
|
||||
_FORCE_INLINE_ RID_Owner<JointBullet> *get_joint_owner() {
|
||||
return &joint_owner;
|
||||
}
|
||||
|
||||
/* SHAPE API */
|
||||
virtual RID shape_create(ShapeType p_shape);
|
||||
virtual void shape_set_data(RID p_shape, const Variant &p_data);
|
||||
virtual ShapeType shape_get_type(RID p_shape) const;
|
||||
virtual Variant shape_get_data(RID p_shape) const;
|
||||
|
||||
/// Not supported
|
||||
virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
|
||||
/// Not supported
|
||||
virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
|
||||
|
||||
/* SPACE API */
|
||||
|
||||
virtual RID space_create();
|
||||
virtual void space_set_active(RID p_space, bool p_active);
|
||||
virtual bool space_is_active(RID p_space) const;
|
||||
|
||||
/// Not supported
|
||||
virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value);
|
||||
/// Not supported
|
||||
virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const;
|
||||
|
||||
virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space);
|
||||
|
||||
virtual void space_set_debug_contacts(RID p_space, int p_max_contacts);
|
||||
virtual Vector<Vector3> space_get_contacts(RID p_space) const;
|
||||
virtual int space_get_contact_count(RID p_space) const;
|
||||
|
||||
/* AREA API */
|
||||
|
||||
/// Bullet Physics Engine not support "Area", this must be handled by the game developer in another way.
|
||||
/// Since godot Physics use the concept of area even to define the main world, the API area_set_param is used to set initial physics world information.
|
||||
/// The API area_set_param is a bit hacky, and allow Godot to set some parameters on Bullet's world, a different use print a warning to console.
|
||||
/// All other APIs returns a warning message if used
|
||||
|
||||
virtual RID area_create();
|
||||
|
||||
virtual void area_set_space(RID p_area, RID p_space);
|
||||
|
||||
virtual RID area_get_space(RID p_area) const;
|
||||
|
||||
virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
|
||||
virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
|
||||
|
||||
virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform());
|
||||
virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape);
|
||||
virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform);
|
||||
virtual int area_get_shape_count(RID p_area) const;
|
||||
virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
|
||||
virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const;
|
||||
virtual void area_remove_shape(RID p_area, int p_shape_idx);
|
||||
virtual void area_clear_shapes(RID p_area);
|
||||
virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled);
|
||||
virtual void area_attach_object_instance_id(RID p_area, ObjectID p_ID);
|
||||
virtual ObjectID area_get_object_instance_id(RID p_area) const;
|
||||
|
||||
/// If you pass as p_area the SpaceBullet you can set some parameters as specified below
|
||||
/// AREA_PARAM_GRAVITY
|
||||
/// AREA_PARAM_GRAVITY_VECTOR
|
||||
/// Otherwise you can set area parameters
|
||||
virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value);
|
||||
virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const;
|
||||
|
||||
virtual void area_set_transform(RID p_area, const Transform &p_transform);
|
||||
virtual Transform area_get_transform(RID p_area) const;
|
||||
|
||||
virtual void area_set_collision_mask(RID p_area, uint32_t p_mask);
|
||||
virtual void area_set_collision_layer(RID p_area, uint32_t p_layer);
|
||||
|
||||
virtual void area_set_monitorable(RID p_area, bool p_monitorable);
|
||||
virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
|
||||
virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
|
||||
virtual void area_set_ray_pickable(RID p_area, bool p_enable);
|
||||
virtual bool area_is_ray_pickable(RID p_area) const;
|
||||
|
||||
/* RIGID BODY API */
|
||||
|
||||
virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false);
|
||||
|
||||
virtual void body_set_space(RID p_body, RID p_space);
|
||||
virtual RID body_get_space(RID p_body) const;
|
||||
|
||||
virtual void body_set_mode(RID p_body, BodyMode p_mode);
|
||||
virtual BodyMode body_get_mode(RID p_body) const;
|
||||
|
||||
virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform());
|
||||
// Not supported, Please remove and add new shape
|
||||
virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape);
|
||||
virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform);
|
||||
|
||||
virtual int body_get_shape_count(RID p_body) const;
|
||||
virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
|
||||
virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
|
||||
|
||||
virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
|
||||
|
||||
virtual void body_remove_shape(RID p_body, int p_shape_idx);
|
||||
virtual void body_clear_shapes(RID p_body);
|
||||
|
||||
// Used for Rigid and Soft Bodies
|
||||
virtual void body_attach_object_instance_id(RID p_body, uint32_t p_ID);
|
||||
virtual uint32_t body_get_object_instance_id(RID p_body) const;
|
||||
|
||||
virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable);
|
||||
virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
|
||||
|
||||
virtual void body_set_collision_layer(RID p_body, uint32_t p_layer);
|
||||
virtual uint32_t body_get_collision_layer(RID p_body) const;
|
||||
|
||||
virtual void body_set_collision_mask(RID p_body, uint32_t p_mask);
|
||||
virtual uint32_t body_get_collision_mask(RID p_body) const;
|
||||
|
||||
/// This is not supported by physics server
|
||||
virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
|
||||
/// This is not supported by physics server
|
||||
virtual uint32_t body_get_user_flags(RID p_body) const;
|
||||
|
||||
virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
|
||||
virtual float body_get_param(RID p_body, BodyParameter p_param) const;
|
||||
|
||||
virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
|
||||
virtual Variant body_get_state(RID p_body, BodyState p_state) const;
|
||||
|
||||
virtual void body_set_applied_force(RID p_body, const Vector3 &p_force);
|
||||
virtual Vector3 body_get_applied_force(RID p_body) const;
|
||||
|
||||
virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque);
|
||||
virtual Vector3 body_get_applied_torque(RID p_body) const;
|
||||
|
||||
virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse);
|
||||
virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse);
|
||||
virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity);
|
||||
|
||||
virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock);
|
||||
virtual BodyAxisLock body_get_axis_lock(RID p_body) const;
|
||||
|
||||
virtual void body_add_collision_exception(RID p_body, RID p_body_b);
|
||||
virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
|
||||
virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
|
||||
|
||||
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
|
||||
virtual int body_get_max_contacts_reported(RID p_body) const;
|
||||
|
||||
virtual void body_set_contacts_reported_depth_threshold(RID p_body, float p_treshold);
|
||||
virtual float body_get_contacts_reported_depth_threshold(RID p_body) const;
|
||||
|
||||
virtual void body_set_omit_force_integration(RID p_body, bool p_omit);
|
||||
virtual bool body_is_omitting_force_integration(RID p_body) const;
|
||||
|
||||
virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant());
|
||||
|
||||
virtual void body_set_ray_pickable(RID p_body, bool p_enable);
|
||||
virtual bool body_is_ray_pickable(RID p_body) const;
|
||||
|
||||
// this function only works on physics process, errors and returns null otherwise
|
||||
virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
|
||||
|
||||
virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
|
||||
|
||||
/* SOFT BODY API */
|
||||
|
||||
virtual RID soft_body_create(bool p_init_sleeping = false);
|
||||
|
||||
virtual void soft_body_set_space(RID p_body, RID p_space);
|
||||
virtual RID soft_body_get_space(RID p_body) const;
|
||||
|
||||
virtual void soft_body_set_trimesh_body_shape(RID p_body, PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num);
|
||||
|
||||
virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer);
|
||||
virtual uint32_t soft_body_get_collision_layer(RID p_body) const;
|
||||
|
||||
virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask);
|
||||
virtual uint32_t soft_body_get_collision_mask(RID p_body) const;
|
||||
|
||||
virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b);
|
||||
virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b);
|
||||
virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
|
||||
|
||||
virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
|
||||
virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const;
|
||||
|
||||
virtual void soft_body_set_transform(RID p_body, const Transform &p_transform);
|
||||
virtual Transform soft_body_get_transform(RID p_body) const;
|
||||
|
||||
virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable);
|
||||
virtual bool soft_body_is_ray_pickable(RID p_body) const;
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
virtual JointType joint_get_type(RID p_joint) const;
|
||||
|
||||
virtual void joint_set_solver_priority(RID p_joint, int p_priority);
|
||||
virtual int joint_get_solver_priority(RID p_joint) const;
|
||||
|
||||
virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
|
||||
|
||||
virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value);
|
||||
virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const;
|
||||
|
||||
virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A);
|
||||
virtual Vector3 pin_joint_get_local_a(RID p_joint) const;
|
||||
|
||||
virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B);
|
||||
virtual Vector3 pin_joint_get_local_b(RID p_joint) const;
|
||||
|
||||
virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B);
|
||||
virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B);
|
||||
|
||||
virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value);
|
||||
virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const;
|
||||
|
||||
virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value);
|
||||
virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const;
|
||||
|
||||
/// Reference frame is A
|
||||
virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
|
||||
|
||||
virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value);
|
||||
virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const;
|
||||
|
||||
/// Reference frame is A
|
||||
virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
|
||||
|
||||
virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value);
|
||||
virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const;
|
||||
|
||||
/// Reference frame is A
|
||||
virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
|
||||
|
||||
virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, float p_value);
|
||||
virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param);
|
||||
|
||||
virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable);
|
||||
virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag);
|
||||
|
||||
/* MISC */
|
||||
|
||||
virtual void free(RID p_rid);
|
||||
|
||||
virtual void set_active(bool p_active) {
|
||||
active = p_active;
|
||||
}
|
||||
|
||||
static bool singleton_isActive() {
|
||||
return static_cast<BulletPhysicsServer *>(get_singleton())->active;
|
||||
}
|
||||
|
||||
bool isActive() {
|
||||
return active;
|
||||
}
|
||||
|
||||
virtual void init();
|
||||
virtual void step(float p_deltaTime);
|
||||
virtual void sync();
|
||||
virtual void flush_queries();
|
||||
virtual void finish();
|
||||
|
||||
virtual int get_process_info(ProcessInfo p_info);
|
||||
|
||||
CollisionObjectBullet *get_collisin_object(RID p_object) const;
|
||||
RigidCollisionObjectBullet *get_rigid_collisin_object(RID p_object) const;
|
||||
|
||||
/// Internal APIs
|
||||
public:
|
||||
};
|
||||
|
||||
#endif
|
94
modules/bullet/bullet_types_converter.cpp
Normal file
94
modules/bullet/bullet_types_converter.cpp
Normal file
|
@ -0,0 +1,94 @@
|
|||
/*************************************************************************/
|
||||
/* bullet_types_converter.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "bullet_types_converter.h"
|
||||
|
||||
// ++ BULLET to GODOT ++++++++++
|
||||
void B_TO_G(btVector3 const &inVal, Vector3 &outVal) {
|
||||
outVal[0] = inVal[0];
|
||||
outVal[1] = inVal[1];
|
||||
outVal[2] = inVal[2];
|
||||
}
|
||||
|
||||
void INVERT_B_TO_G(btVector3 const &inVal, Vector3 &outVal) {
|
||||
outVal[0] = inVal[0] != 0. ? 1. / inVal[0] : 0.;
|
||||
outVal[1] = inVal[1] != 0. ? 1. / inVal[1] : 0.;
|
||||
outVal[2] = inVal[2] != 0. ? 1. / inVal[2] : 0.;
|
||||
}
|
||||
|
||||
void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
|
||||
B_TO_G(inVal[0], outVal[0]);
|
||||
B_TO_G(inVal[1], outVal[1]);
|
||||
B_TO_G(inVal[2], outVal[2]);
|
||||
}
|
||||
|
||||
void INVERT_B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
|
||||
INVERT_B_TO_G(inVal[0], outVal[0]);
|
||||
INVERT_B_TO_G(inVal[1], outVal[1]);
|
||||
INVERT_B_TO_G(inVal[2], outVal[2]);
|
||||
}
|
||||
|
||||
void B_TO_G(btTransform const &inVal, Transform &outVal) {
|
||||
B_TO_G(inVal.getBasis(), outVal.basis);
|
||||
B_TO_G(inVal.getOrigin(), outVal.origin);
|
||||
}
|
||||
|
||||
// ++ GODOT to BULLET ++++++++++
|
||||
void G_TO_B(Vector3 const &inVal, btVector3 &outVal) {
|
||||
outVal[0] = inVal[0];
|
||||
outVal[1] = inVal[1];
|
||||
outVal[2] = inVal[2];
|
||||
}
|
||||
|
||||
void INVERT_G_TO_B(Vector3 const &inVal, btVector3 &outVal) {
|
||||
outVal[0] = inVal[0] != 0. ? 1. / inVal[0] : 0.;
|
||||
outVal[1] = inVal[1] != 0. ? 1. / inVal[1] : 0.;
|
||||
outVal[2] = inVal[2] != 0. ? 1. / inVal[2] : 0.;
|
||||
}
|
||||
|
||||
void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
|
||||
G_TO_B(inVal[0], outVal[0]);
|
||||
G_TO_B(inVal[1], outVal[1]);
|
||||
G_TO_B(inVal[2], outVal[2]);
|
||||
}
|
||||
|
||||
void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
|
||||
INVERT_G_TO_B(inVal[0], outVal[0]);
|
||||
INVERT_G_TO_B(inVal[1], outVal[1]);
|
||||
INVERT_G_TO_B(inVal[2], outVal[2]);
|
||||
}
|
||||
|
||||
void G_TO_B(Transform const &inVal, btTransform &outVal) {
|
||||
G_TO_B(inVal.basis, outVal.getBasis());
|
||||
G_TO_B(inVal.origin, outVal.getOrigin());
|
||||
}
|
57
modules/bullet/bullet_types_converter.h
Normal file
57
modules/bullet/bullet_types_converter.h
Normal file
|
@ -0,0 +1,57 @@
|
|||
/*************************************************************************/
|
||||
/* bullet_types_converter.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BULLET_TYPES_CONVERTER_H
|
||||
#define BULLET_TYPES_CONVERTER_H
|
||||
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "core/math/matrix3.h"
|
||||
#include "core/math/transform.h"
|
||||
#include "core/math/vector3.h"
|
||||
#include "core/typedefs.h"
|
||||
|
||||
// Bullet to Godot
|
||||
extern void B_TO_G(btVector3 const &inVal, Vector3 &outVal);
|
||||
extern void INVERT_B_TO_G(btVector3 const &inVal, Vector3 &outVal);
|
||||
extern void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal);
|
||||
extern void INVERT_B_TO_G(btMatrix3x3 const &inVal, Basis &outVal);
|
||||
extern void B_TO_G(btTransform const &inVal, Transform &outVal);
|
||||
|
||||
// Godot TO Bullet
|
||||
extern void G_TO_B(Vector3 const &inVal, btVector3 &outVal);
|
||||
extern void INVERT_G_TO_B(Vector3 const &inVal, btVector3 &outVal);
|
||||
extern void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
|
||||
extern void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
|
||||
extern void G_TO_B(Transform const &inVal, btTransform &outVal);
|
||||
|
||||
#endif
|
44
modules/bullet/bullet_utilities.h
Normal file
44
modules/bullet/bullet_utilities.h
Normal file
|
@ -0,0 +1,44 @@
|
|||
/*************************************************************************/
|
||||
/* bullet_utilities.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BULLET_UTILITIES_H
|
||||
#define BULLET_UTILITIES_H
|
||||
|
||||
#pragma once
|
||||
|
||||
#define bulletnew(cl) \
|
||||
new cl
|
||||
|
||||
#define bulletdelete(cl) \
|
||||
delete cl; \
|
||||
cl = NULL;
|
||||
|
||||
#endif
|
316
modules/bullet/collision_object_bullet.cpp
Normal file
316
modules/bullet/collision_object_bullet.cpp
Normal file
|
@ -0,0 +1,316 @@
|
|||
/*************************************************************************/
|
||||
/* collision_object_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "collision_object_bullet.h"
|
||||
#include "area_bullet.h"
|
||||
#include "btBulletCollisionCommon.h"
|
||||
#include "bullet_physics_server.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "shape_bullet.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
#define enableDynamicAabbTree true
|
||||
#define initialChildCapacity 1
|
||||
|
||||
CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
|
||||
|
||||
void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_transform) {
|
||||
G_TO_B(p_transform, transform);
|
||||
}
|
||||
void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_transform) {
|
||||
transform = p_transform;
|
||||
}
|
||||
|
||||
CollisionObjectBullet::CollisionObjectBullet(Type p_type)
|
||||
: RIDBullet(), space(NULL), type(p_type), collisionsEnabled(true), m_isStatic(false), bt_collision_object(NULL), body_scale(1., 1., 1.) {}
|
||||
|
||||
CollisionObjectBullet::~CollisionObjectBullet() {
|
||||
// Remove all overlapping
|
||||
for (int i = areasOverlapped.size() - 1; 0 <= i; --i) {
|
||||
areasOverlapped[i]->remove_overlapping_instantly(this);
|
||||
}
|
||||
// not required
|
||||
// areasOverlapped.clear();
|
||||
|
||||
destroyBulletCollisionObject();
|
||||
}
|
||||
|
||||
bool equal(real_t first, real_t second) {
|
||||
return Math::abs(first - second) <= 0.001f;
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::set_body_scale(const Vector3 &p_new_scale) {
|
||||
if (!equal(p_new_scale[0], body_scale[0]) || !equal(p_new_scale[1], body_scale[1]) || !equal(p_new_scale[2], body_scale[2])) {
|
||||
G_TO_B(p_new_scale, body_scale);
|
||||
on_body_scale_changed();
|
||||
}
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::on_body_scale_changed() {
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::destroyBulletCollisionObject() {
|
||||
bulletdelete(bt_collision_object);
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::setupBulletCollisionObject(btCollisionObject *p_collisionObject) {
|
||||
bt_collision_object = p_collisionObject;
|
||||
bt_collision_object->setUserPointer(this);
|
||||
bt_collision_object->setUserIndex(type);
|
||||
// Force the enabling of collision and avoid problems
|
||||
set_collision_enabled(collisionsEnabled);
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
|
||||
exceptions.insert(p_ignoreCollisionObject->get_self());
|
||||
bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, true);
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
|
||||
exceptions.erase(p_ignoreCollisionObject->get_self());
|
||||
bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, false);
|
||||
}
|
||||
|
||||
bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const {
|
||||
return !bt_collision_object->checkCollideWithOverride(p_otherCollisionObject->bt_collision_object);
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::set_collision_enabled(bool p_enabled) {
|
||||
collisionsEnabled = p_enabled;
|
||||
if (collisionsEnabled) {
|
||||
bt_collision_object->setCollisionFlags(bt_collision_object->getCollisionFlags() & (~btCollisionObject::CF_NO_CONTACT_RESPONSE));
|
||||
} else {
|
||||
bt_collision_object->setCollisionFlags(bt_collision_object->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
|
||||
}
|
||||
}
|
||||
|
||||
bool CollisionObjectBullet::is_collisions_response_enabled() {
|
||||
return collisionsEnabled;
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::notify_new_overlap(AreaBullet *p_area) {
|
||||
areasOverlapped.push_back(p_area);
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::on_exit_area(AreaBullet *p_area) {
|
||||
areasOverlapped.erase(p_area);
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::set_godot_object_flags(int flags) {
|
||||
bt_collision_object->setUserIndex2(flags);
|
||||
}
|
||||
|
||||
int CollisionObjectBullet::get_godot_object_flags() const {
|
||||
return bt_collision_object->getUserIndex2();
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::set_transform(const Transform &p_global_transform) {
|
||||
|
||||
btTransform btTrans;
|
||||
Basis decomposed_basis;
|
||||
|
||||
Vector3 decomposed_scale = p_global_transform.get_basis().rotref_posscale_decomposition(decomposed_basis);
|
||||
|
||||
G_TO_B(p_global_transform.get_origin(), btTrans.getOrigin());
|
||||
G_TO_B(decomposed_basis, btTrans.getBasis());
|
||||
|
||||
set_body_scale(decomposed_scale);
|
||||
set_transform__bullet(btTrans);
|
||||
}
|
||||
|
||||
Transform CollisionObjectBullet::get_transform() const {
|
||||
Transform t;
|
||||
B_TO_G(get_transform__bullet(), t);
|
||||
return t;
|
||||
}
|
||||
|
||||
void CollisionObjectBullet::set_transform__bullet(const btTransform &p_global_transform) {
|
||||
bt_collision_object->setWorldTransform(p_global_transform);
|
||||
}
|
||||
|
||||
const btTransform &CollisionObjectBullet::get_transform__bullet() const {
|
||||
return bt_collision_object->getWorldTransform();
|
||||
}
|
||||
|
||||
RigidCollisionObjectBullet::RigidCollisionObjectBullet(Type p_type)
|
||||
: CollisionObjectBullet(p_type), compoundShape(bulletnew(btCompoundShape(enableDynamicAabbTree, initialChildCapacity))) {
|
||||
}
|
||||
|
||||
RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
|
||||
remove_all_shapes(true);
|
||||
bt_collision_object->setCollisionShape(NULL);
|
||||
bulletdelete(compoundShape);
|
||||
}
|
||||
|
||||
/* Not used
|
||||
void RigidCollisionObjectBullet::_internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) {
|
||||
bool at_least_one_was_changed = false;
|
||||
btTransform old_transf;
|
||||
// Inverse because I need remove the shapes
|
||||
// Fetch all shapes to be sure to remove all shapes
|
||||
for (int i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
|
||||
if (compoundShape->getChildShape(i) == p_old_shape) {
|
||||
|
||||
old_transf = compoundShape->getChildTransform(i);
|
||||
compoundShape->removeChildShapeByIndex(i);
|
||||
compoundShape->addChildShape(old_transf, p_new_shape);
|
||||
at_least_one_was_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (at_least_one_was_changed) {
|
||||
on_shapes_changed();
|
||||
}
|
||||
}*/
|
||||
|
||||
void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform &p_transform) {
|
||||
shapes.push_back(ShapeWrapper(p_shape, p_transform, true));
|
||||
p_shape->add_owner(this);
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
|
||||
ShapeWrapper &shp = shapes[p_index];
|
||||
shp.shape->remove_owner(this);
|
||||
p_shape->add_owner(this);
|
||||
shp.shape = p_shape;
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
|
||||
ERR_FAIL_INDEX(p_index, get_shape_count());
|
||||
|
||||
shapes[p_index].set_transform(p_transform);
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
|
||||
// Remove the shape, all the times it appears
|
||||
// Reverse order required for delete.
|
||||
for (int i = shapes.size() - 1; 0 <= i; --i) {
|
||||
if (p_shape == shapes[i].shape) {
|
||||
internal_shape_destroy(i);
|
||||
shapes.remove(i);
|
||||
}
|
||||
}
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::remove_shape(int p_index) {
|
||||
ERR_FAIL_INDEX(p_index, get_shape_count());
|
||||
internal_shape_destroy(p_index);
|
||||
shapes.remove(p_index);
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBody) {
|
||||
// Reverse order required for delete.
|
||||
for (int i = shapes.size() - 1; 0 <= i; --i) {
|
||||
internal_shape_destroy(i, p_permanentlyFromThisBody);
|
||||
}
|
||||
shapes.clear();
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
int RigidCollisionObjectBullet::get_shape_count() const {
|
||||
return shapes.size();
|
||||
}
|
||||
|
||||
ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
|
||||
return shapes[p_index].shape;
|
||||
}
|
||||
|
||||
btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
|
||||
return shapes[p_index].bt_shape;
|
||||
}
|
||||
|
||||
Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
|
||||
Transform trs;
|
||||
B_TO_G(shapes[p_index].transform, trs);
|
||||
return trs;
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::on_shape_changed(const ShapeBullet *const p_shape) {
|
||||
const int size = shapes.size();
|
||||
for (int i = 0; i < size; ++i) {
|
||||
if (shapes[i].shape == p_shape) {
|
||||
bulletdelete(shapes[i].bt_shape);
|
||||
}
|
||||
}
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::on_shapes_changed() {
|
||||
int i;
|
||||
// Remove all shapes, reverse order for performance reason (Array resize)
|
||||
for (i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
|
||||
compoundShape->removeChildShapeByIndex(i);
|
||||
}
|
||||
|
||||
// Insert all shapes
|
||||
ShapeWrapper *shpWrapper;
|
||||
const int size = shapes.size();
|
||||
for (i = 0; i < size; ++i) {
|
||||
shpWrapper = &shapes[i];
|
||||
if (!shpWrapper->bt_shape) {
|
||||
shpWrapper->bt_shape = shpWrapper->shape->create_bt_shape();
|
||||
}
|
||||
if (shpWrapper->active) {
|
||||
compoundShape->addChildShape(shpWrapper->transform, shpWrapper->bt_shape);
|
||||
} else {
|
||||
compoundShape->addChildShape(shpWrapper->transform, BulletPhysicsServer::get_empty_shape());
|
||||
}
|
||||
}
|
||||
|
||||
compoundShape->setLocalScaling(body_scale);
|
||||
compoundShape->recalculateLocalAabb();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
|
||||
shapes[p_index].active = !p_disabled;
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
|
||||
return !shapes[p_index].active;
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::on_body_scale_changed() {
|
||||
CollisionObjectBullet::on_body_scale_changed();
|
||||
on_shapes_changed();
|
||||
}
|
||||
|
||||
void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) {
|
||||
ShapeWrapper &shp = shapes[p_index];
|
||||
shp.shape->remove_owner(this, p_permanentlyFromThisBody);
|
||||
bulletdelete(shp.bt_shape);
|
||||
}
|
234
modules/bullet/collision_object_bullet.h
Normal file
234
modules/bullet/collision_object_bullet.h
Normal file
|
@ -0,0 +1,234 @@
|
|||
/*************************************************************************/
|
||||
/* collision_object_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef COLLISION_OBJECT_BULLET_H
|
||||
#define COLLISION_OBJECT_BULLET_H
|
||||
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "core/vset.h"
|
||||
#include "object.h"
|
||||
#include "shape_owner_bullet.h"
|
||||
#include "transform.h"
|
||||
#include "vector3.h"
|
||||
|
||||
class AreaBullet;
|
||||
class ShapeBullet;
|
||||
class btCollisionObject;
|
||||
class btCompoundShape;
|
||||
class btCollisionShape;
|
||||
class SpaceBullet;
|
||||
|
||||
class CollisionObjectBullet : public RIDBullet {
|
||||
public:
|
||||
enum GodotObjectFlags {
|
||||
GOF_IS_MONITORING_AREA = 1 << 0
|
||||
// FLAG2 = 1 << 1,
|
||||
// FLAG3 = 1 << 2,
|
||||
// FLAG4 = 1 << 3,
|
||||
// FLAG5 = 1 << 4,
|
||||
// FLAG6 = 1 << 5
|
||||
// etc..
|
||||
};
|
||||
enum Type {
|
||||
TYPE_AREA = 0,
|
||||
TYPE_RIGID_BODY,
|
||||
TYPE_SOFT_BODY,
|
||||
TYPE_KINEMATIC_GHOST_BODY
|
||||
};
|
||||
|
||||
struct ShapeWrapper {
|
||||
ShapeBullet *shape;
|
||||
btCollisionShape *bt_shape;
|
||||
btTransform transform;
|
||||
bool active;
|
||||
|
||||
ShapeWrapper()
|
||||
: shape(NULL), bt_shape(NULL), active(true) {}
|
||||
|
||||
ShapeWrapper(ShapeBullet *p_shape, const btTransform &p_transform, bool p_active)
|
||||
: shape(p_shape), bt_shape(NULL), active(p_active) {
|
||||
set_transform(p_transform);
|
||||
}
|
||||
|
||||
ShapeWrapper(ShapeBullet *p_shape, const Transform &p_transform, bool p_active)
|
||||
: shape(p_shape), bt_shape(NULL), active(p_active) {
|
||||
set_transform(p_transform);
|
||||
}
|
||||
~ShapeWrapper();
|
||||
|
||||
ShapeWrapper(const ShapeWrapper &otherShape) {
|
||||
operator=(otherShape);
|
||||
}
|
||||
|
||||
void operator=(const ShapeWrapper &otherShape) {
|
||||
shape = otherShape.shape;
|
||||
bt_shape = otherShape.bt_shape;
|
||||
transform = otherShape.transform;
|
||||
active = otherShape.active;
|
||||
}
|
||||
|
||||
void set_transform(const Transform &p_transform);
|
||||
void set_transform(const btTransform &p_transform);
|
||||
};
|
||||
|
||||
protected:
|
||||
Type type;
|
||||
ObjectID instance_id;
|
||||
uint32_t collisionLayer;
|
||||
uint32_t collisionMask;
|
||||
bool collisionsEnabled;
|
||||
bool m_isStatic;
|
||||
bool ray_pickable;
|
||||
btCollisionObject *bt_collision_object;
|
||||
btVector3 body_scale;
|
||||
SpaceBullet *space;
|
||||
|
||||
VSet<RID> exceptions;
|
||||
|
||||
/// This array is used to know all areas where this Object is overlapped in
|
||||
/// New area is added when overlap with new area (AreaBullet::addOverlap), then is removed when it exit (CollisionObjectBullet::onExitArea)
|
||||
/// This array is used mainly to know which area hold the pointer of this object
|
||||
Vector<AreaBullet *> areasOverlapped;
|
||||
|
||||
public:
|
||||
CollisionObjectBullet(Type p_type);
|
||||
virtual ~CollisionObjectBullet();
|
||||
|
||||
Type getType() { return type; }
|
||||
|
||||
protected:
|
||||
void destroyBulletCollisionObject();
|
||||
void setupBulletCollisionObject(btCollisionObject *p_collisionObject);
|
||||
|
||||
public:
|
||||
_FORCE_INLINE_ btCollisionObject *get_bt_collision_object() { return bt_collision_object; }
|
||||
|
||||
_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
|
||||
_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
|
||||
|
||||
_FORCE_INLINE_ bool is_static() const { return m_isStatic; }
|
||||
|
||||
_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
|
||||
_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
|
||||
|
||||
void set_body_scale(const Vector3 &p_new_scale);
|
||||
virtual void on_body_scale_changed();
|
||||
|
||||
void add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
|
||||
void remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
|
||||
bool has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const;
|
||||
_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
|
||||
|
||||
_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
|
||||
collisionLayer = p_layer;
|
||||
on_collision_filters_change();
|
||||
}
|
||||
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collisionLayer; }
|
||||
|
||||
_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
|
||||
collisionMask = p_mask;
|
||||
on_collision_filters_change();
|
||||
}
|
||||
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collisionMask; }
|
||||
|
||||
virtual void on_collision_filters_change() = 0;
|
||||
|
||||
_FORCE_INLINE_ bool test_collision_mask(CollisionObjectBullet *p_other) const {
|
||||
return collisionLayer & p_other->collisionMask || p_other->collisionLayer & collisionMask;
|
||||
}
|
||||
|
||||
virtual void reload_body() = 0;
|
||||
virtual void set_space(SpaceBullet *p_space) = 0;
|
||||
_FORCE_INLINE_ SpaceBullet *get_space() const { return space; }
|
||||
/// This is an event that is called when a collision checker starts
|
||||
virtual void on_collision_checker_start() = 0;
|
||||
|
||||
virtual void dispatch_callbacks() = 0;
|
||||
|
||||
void set_collision_enabled(bool p_enabled);
|
||||
bool is_collisions_response_enabled();
|
||||
|
||||
void notify_new_overlap(AreaBullet *p_area);
|
||||
virtual void on_enter_area(AreaBullet *p_area) = 0;
|
||||
virtual void on_exit_area(AreaBullet *p_area);
|
||||
|
||||
/// GodotObjectFlags
|
||||
void set_godot_object_flags(int flags);
|
||||
int get_godot_object_flags() const;
|
||||
|
||||
void set_transform(const Transform &p_global_transform);
|
||||
Transform get_transform() const;
|
||||
virtual void set_transform__bullet(const btTransform &p_global_transform);
|
||||
virtual const btTransform &get_transform__bullet() const;
|
||||
};
|
||||
|
||||
class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
|
||||
protected:
|
||||
/// This is required to combine some shapes together.
|
||||
/// Since Godot allow to have multiple shapes for each body with custom relative location,
|
||||
/// each body will attach the shapes using this class even if there is only one shape.
|
||||
btCompoundShape *compoundShape;
|
||||
Vector<ShapeWrapper> shapes;
|
||||
|
||||
public:
|
||||
RigidCollisionObjectBullet(Type p_type);
|
||||
~RigidCollisionObjectBullet();
|
||||
|
||||
_FORCE_INLINE_ const Vector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
|
||||
|
||||
/// This is used to set new shape or replace existing
|
||||
//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
|
||||
void add_shape(ShapeBullet *p_shape, const Transform &p_transform = Transform());
|
||||
void set_shape(int p_index, ShapeBullet *p_shape);
|
||||
void set_shape_transform(int p_index, const Transform &p_transform);
|
||||
virtual void remove_shape(ShapeBullet *p_shape);
|
||||
void remove_shape(int p_index);
|
||||
void remove_all_shapes(bool p_permanentlyFromThisBody = false);
|
||||
|
||||
virtual void on_shape_changed(const ShapeBullet *const p_shape);
|
||||
virtual void on_shapes_changed();
|
||||
|
||||
_FORCE_INLINE_ btCompoundShape *get_compound_shape() const { return compoundShape; }
|
||||
int get_shape_count() const;
|
||||
ShapeBullet *get_shape(int p_index) const;
|
||||
btCollisionShape *get_bt_shape(int p_index) const;
|
||||
Transform get_shape_transform(int p_index) const;
|
||||
|
||||
void set_shape_disabled(int p_index, bool p_disabled);
|
||||
bool is_shape_disabled(int p_index);
|
||||
|
||||
virtual void on_body_scale_changed();
|
||||
|
||||
private:
|
||||
void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false);
|
||||
};
|
||||
|
||||
#endif
|
111
modules/bullet/cone_twist_joint_bullet.cpp
Normal file
111
modules/bullet/cone_twist_joint_bullet.cpp
Normal file
|
@ -0,0 +1,111 @@
|
|||
/*************************************************************************/
|
||||
/* cone_twist_joint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "cone_twist_joint_bullet.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
ConeTwistJointBullet::ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame)
|
||||
: JointBullet() {
|
||||
btTransform btFrameA;
|
||||
G_TO_B(rbAFrame, btFrameA);
|
||||
if (rbB) {
|
||||
btTransform btFrameB;
|
||||
G_TO_B(rbBFrame, btFrameB);
|
||||
coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
|
||||
} else {
|
||||
coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), btFrameA));
|
||||
}
|
||||
setup(coneConstraint);
|
||||
}
|
||||
|
||||
void ConeTwistJointBullet::set_angular_only(bool angularOnly) {
|
||||
coneConstraint->setAngularOnly(angularOnly);
|
||||
}
|
||||
|
||||
void ConeTwistJointBullet::set_limit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness, real_t _biasFactor, real_t _relaxationFactor) {
|
||||
coneConstraint->setLimit(_swingSpan1, _swingSpan2, _twistSpan, _softness, _biasFactor, _relaxationFactor);
|
||||
}
|
||||
|
||||
int ConeTwistJointBullet::get_solve_twist_limit() {
|
||||
return coneConstraint->getSolveTwistLimit();
|
||||
}
|
||||
|
||||
int ConeTwistJointBullet::get_solve_swing_limit() {
|
||||
return coneConstraint->getSolveSwingLimit();
|
||||
}
|
||||
|
||||
real_t ConeTwistJointBullet::get_twist_limit_sign() {
|
||||
return coneConstraint->getTwistLimitSign();
|
||||
}
|
||||
|
||||
void ConeTwistJointBullet::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN:
|
||||
coneConstraint->setLimit(5, p_value);
|
||||
coneConstraint->setLimit(4, p_value);
|
||||
break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN:
|
||||
coneConstraint->setLimit(3, p_value);
|
||||
break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_BIAS:
|
||||
coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), p_value, coneConstraint->getRelaxationFactor());
|
||||
break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS:
|
||||
coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), p_value, coneConstraint->getBiasFactor(), coneConstraint->getRelaxationFactor());
|
||||
break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION:
|
||||
coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), coneConstraint->getBiasFactor(), p_value);
|
||||
break;
|
||||
default:
|
||||
WARN_PRINT("This parameter is not supported by Bullet engine");
|
||||
}
|
||||
}
|
||||
|
||||
real_t ConeTwistJointBullet::get_param(PhysicsServer::ConeTwistJointParam p_param) const {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN:
|
||||
return coneConstraint->getSwingSpan1();
|
||||
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN:
|
||||
return coneConstraint->getTwistSpan();
|
||||
case PhysicsServer::CONE_TWIST_JOINT_BIAS:
|
||||
return coneConstraint->getBiasFactor();
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS:
|
||||
return coneConstraint->getLimitSoftness();
|
||||
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION:
|
||||
return coneConstraint->getRelaxationFactor();
|
||||
default:
|
||||
WARN_PRINT("This parameter is not supported by Bullet engine");
|
||||
return 0;
|
||||
}
|
||||
}
|
58
modules/bullet/cone_twist_joint_bullet.h
Normal file
58
modules/bullet/cone_twist_joint_bullet.h
Normal file
|
@ -0,0 +1,58 @@
|
|||
/*************************************************************************/
|
||||
/* cone_twist_joint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef CONE_TWIST_JOINT_BULLET_H
|
||||
#define CONE_TWIST_JOINT_BULLET_H
|
||||
|
||||
#include "joint_bullet.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
|
||||
class ConeTwistJointBullet : public JointBullet {
|
||||
class btConeTwistConstraint *coneConstraint;
|
||||
|
||||
public:
|
||||
ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame);
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
|
||||
|
||||
void set_angular_only(bool angularOnly);
|
||||
|
||||
void set_limit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f);
|
||||
int get_solve_twist_limit();
|
||||
|
||||
int get_solve_swing_limit();
|
||||
real_t get_twist_limit_sign();
|
||||
|
||||
void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value);
|
||||
real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const;
|
||||
};
|
||||
#endif
|
6
modules/bullet/config.py
Normal file
6
modules/bullet/config.py
Normal file
|
@ -0,0 +1,6 @@
|
|||
def can_build(platform):
|
||||
return True
|
||||
|
||||
def configure(env):
|
||||
pass
|
||||
|
50
modules/bullet/constraint_bullet.cpp
Normal file
50
modules/bullet/constraint_bullet.cpp
Normal file
|
@ -0,0 +1,50 @@
|
|||
/*************************************************************************/
|
||||
/* constraint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "constraint_bullet.h"
|
||||
#include "collision_object_bullet.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
ConstraintBullet::ConstraintBullet()
|
||||
: space(NULL), constraint(NULL) {}
|
||||
|
||||
void ConstraintBullet::setup(btTypedConstraint *p_constraint) {
|
||||
constraint = p_constraint;
|
||||
constraint->setUserConstraintPtr(this);
|
||||
}
|
||||
|
||||
void ConstraintBullet::set_space(SpaceBullet *p_space) {
|
||||
space = p_space;
|
||||
}
|
||||
|
||||
void ConstraintBullet::destroy_internal_constraint() {
|
||||
space->remove_constraint(this);
|
||||
}
|
64
modules/bullet/constraint_bullet.h
Normal file
64
modules/bullet/constraint_bullet.h
Normal file
|
@ -0,0 +1,64 @@
|
|||
/*************************************************************************/
|
||||
/* constraint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef CONSTRAINT_BULLET_H
|
||||
#define CONSTRAINT_BULLET_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "rid_bullet.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
class SpaceBullet;
|
||||
class btTypedConstraint;
|
||||
|
||||
class ConstraintBullet : public RIDBullet {
|
||||
|
||||
protected:
|
||||
SpaceBullet *space;
|
||||
btTypedConstraint *constraint;
|
||||
|
||||
public:
|
||||
ConstraintBullet();
|
||||
|
||||
virtual void setup(btTypedConstraint *p_constraint);
|
||||
virtual void set_space(SpaceBullet *p_space);
|
||||
virtual void destroy_internal_constraint();
|
||||
|
||||
public:
|
||||
virtual ~ConstraintBullet() {
|
||||
bulletdelete(constraint);
|
||||
constraint = NULL;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ btTypedConstraint *get_bt_constraint() { return constraint; }
|
||||
};
|
||||
#endif
|
241
modules/bullet/generic_6dof_joint_bullet.cpp
Normal file
241
modules/bullet/generic_6dof_joint_bullet.cpp
Normal file
|
@ -0,0 +1,241 @@
|
|||
/*************************************************************************/
|
||||
/* generic_6dof_joint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "generic_6dof_joint_bullet.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
Generic6DOFJointBullet::Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA)
|
||||
: JointBullet() {
|
||||
|
||||
btTransform btFrameA;
|
||||
G_TO_B(frameInA, btFrameA);
|
||||
|
||||
if (rbB) {
|
||||
btTransform btFrameB;
|
||||
G_TO_B(frameInB, btFrameB);
|
||||
|
||||
sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, useLinearReferenceFrameA));
|
||||
} else {
|
||||
sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), btFrameA, useLinearReferenceFrameA));
|
||||
}
|
||||
|
||||
setup(sixDOFConstraint);
|
||||
}
|
||||
|
||||
Transform Generic6DOFJointBullet::getFrameOffsetA() const {
|
||||
btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
|
||||
Transform gTrs;
|
||||
B_TO_G(btTrs, gTrs);
|
||||
return gTrs;
|
||||
}
|
||||
|
||||
Transform Generic6DOFJointBullet::getFrameOffsetB() const {
|
||||
btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
|
||||
Transform gTrs;
|
||||
B_TO_G(btTrs, gTrs);
|
||||
return gTrs;
|
||||
}
|
||||
|
||||
Transform Generic6DOFJointBullet::getFrameOffsetA() {
|
||||
btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
|
||||
Transform gTrs;
|
||||
B_TO_G(btTrs, gTrs);
|
||||
return gTrs;
|
||||
}
|
||||
|
||||
Transform Generic6DOFJointBullet::getFrameOffsetB() {
|
||||
btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
|
||||
Transform gTrs;
|
||||
B_TO_G(btTrs, gTrs);
|
||||
return gTrs;
|
||||
}
|
||||
|
||||
void Generic6DOFJointBullet::set_linear_lower_limit(const Vector3 &linearLower) {
|
||||
btVector3 btVec;
|
||||
G_TO_B(linearLower, btVec);
|
||||
sixDOFConstraint->setLinearLowerLimit(btVec);
|
||||
}
|
||||
|
||||
void Generic6DOFJointBullet::set_linear_upper_limit(const Vector3 &linearUpper) {
|
||||
btVector3 btVec;
|
||||
G_TO_B(linearUpper, btVec);
|
||||
sixDOFConstraint->setLinearUpperLimit(btVec);
|
||||
}
|
||||
|
||||
void Generic6DOFJointBullet::set_angular_lower_limit(const Vector3 &angularLower) {
|
||||
btVector3 btVec;
|
||||
G_TO_B(angularLower, btVec);
|
||||
sixDOFConstraint->setAngularLowerLimit(btVec);
|
||||
}
|
||||
|
||||
void Generic6DOFJointBullet::set_angular_upper_limit(const Vector3 &angularUpper) {
|
||||
btVector3 btVec;
|
||||
G_TO_B(angularUpper, btVec);
|
||||
sixDOFConstraint->setAngularUpperLimit(btVec);
|
||||
}
|
||||
|
||||
void Generic6DOFJointBullet::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) {
|
||||
ERR_FAIL_INDEX(p_axis, 3);
|
||||
switch (p_param) {
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
|
||||
sixDOFConstraint->getTranslationalLimitMotor()->m_lowerLimit[p_axis] = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
|
||||
sixDOFConstraint->getTranslationalLimitMotor()->m_upperLimit[p_axis] = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS:
|
||||
sixDOFConstraint->getTranslationalLimitMotor()->m_limitSoftness = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION:
|
||||
sixDOFConstraint->getTranslationalLimitMotor()->m_restitution = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING:
|
||||
sixDOFConstraint->getTranslationalLimitMotor()->m_damping = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_loLimit = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_hiLimit = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_limitSoftness = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_damping = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity = p_value;
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce = p_value;
|
||||
break;
|
||||
default:
|
||||
WARN_PRINT("This parameter is not supported");
|
||||
}
|
||||
}
|
||||
|
||||
real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const {
|
||||
ERR_FAIL_INDEX_V(p_axis, 3, 0.);
|
||||
switch (p_param) {
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
|
||||
return sixDOFConstraint->getTranslationalLimitMotor()->m_lowerLimit[p_axis];
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
|
||||
return sixDOFConstraint->getTranslationalLimitMotor()->m_upperLimit[p_axis];
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS:
|
||||
return sixDOFConstraint->getTranslationalLimitMotor()->m_limitSoftness;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION:
|
||||
return sixDOFConstraint->getTranslationalLimitMotor()->m_restitution;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING:
|
||||
return sixDOFConstraint->getTranslationalLimitMotor()->m_damping;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_loLimit;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_hiLimit;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_limitSoftness;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_damping;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce;
|
||||
default:
|
||||
WARN_PRINT("This parameter is not supported");
|
||||
return 0.;
|
||||
}
|
||||
}
|
||||
|
||||
void Generic6DOFJointBullet::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
|
||||
ERR_FAIL_INDEX(p_axis, 3);
|
||||
switch (p_flag) {
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
|
||||
if (p_value) {
|
||||
if (!get_flag(p_axis, p_flag)) // avoid overwrite, if limited
|
||||
sixDOFConstraint->setLimit(p_axis, 0, 0); // Limited
|
||||
} else {
|
||||
if (get_flag(p_axis, p_flag)) // avoid overwrite, if free
|
||||
sixDOFConstraint->setLimit(p_axis, 0, -1); // Free
|
||||
}
|
||||
break;
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
|
||||
int angularAxis = 3 + p_axis;
|
||||
if (p_value) {
|
||||
if (!get_flag(p_axis, p_flag)) // avoid overwrite, if Limited
|
||||
sixDOFConstraint->setLimit(angularAxis, 0, 0); // Limited
|
||||
} else {
|
||||
if (get_flag(p_axis, p_flag)) // avoid overwrite, if free
|
||||
sixDOFConstraint->setLimit(angularAxis, 0, -1); // Free
|
||||
}
|
||||
break;
|
||||
}
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
|
||||
//sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] = p_value;
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor = p_value;
|
||||
break;
|
||||
default:
|
||||
WARN_PRINT("This flag is not supported by Bullet engine");
|
||||
}
|
||||
}
|
||||
|
||||
bool Generic6DOFJointBullet::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
|
||||
ERR_FAIL_INDEX_V(p_axis, 3, false);
|
||||
switch (p_flag) {
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
|
||||
return sixDOFConstraint->getTranslationalLimitMotor()->isLimited(p_axis);
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT:
|
||||
return sixDOFConstraint->getRotationalLimitMotor(p_axis)->isLimited();
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
|
||||
return //sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] &&
|
||||
sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor;
|
||||
default:
|
||||
WARN_PRINT("This flag is not supported by Bullet engine");
|
||||
return false;
|
||||
}
|
||||
}
|
65
modules/bullet/generic_6dof_joint_bullet.h
Normal file
65
modules/bullet/generic_6dof_joint_bullet.h
Normal file
|
@ -0,0 +1,65 @@
|
|||
/*************************************************************************/
|
||||
/* generic_6dof_joint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef GENERIC_6DOF_JOINT_BULLET_H
|
||||
#define GENERIC_6DOF_JOINT_BULLET_H
|
||||
|
||||
#include "joint_bullet.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
|
||||
class Generic6DOFJointBullet : public JointBullet {
|
||||
class btGeneric6DofConstraint *sixDOFConstraint;
|
||||
|
||||
public:
|
||||
Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
|
||||
|
||||
Transform getFrameOffsetA() const;
|
||||
Transform getFrameOffsetB() const;
|
||||
Transform getFrameOffsetA();
|
||||
Transform getFrameOffsetB();
|
||||
|
||||
void set_linear_lower_limit(const Vector3 &linearLower);
|
||||
void set_linear_upper_limit(const Vector3 &linearUpper);
|
||||
|
||||
void set_angular_lower_limit(const Vector3 &angularLower);
|
||||
void set_angular_upper_limit(const Vector3 &angularUpper);
|
||||
|
||||
void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value);
|
||||
real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const;
|
||||
|
||||
void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
|
||||
bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const;
|
||||
};
|
||||
|
||||
#endif
|
91
modules/bullet/godot_collision_configuration.cpp
Normal file
91
modules/bullet/godot_collision_configuration.cpp
Normal file
|
@ -0,0 +1,91 @@
|
|||
/*************************************************************************/
|
||||
/* godot_collision_configuration.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "godot_collision_configuration.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
|
||||
#include "godot_ray_world_algorithm.h"
|
||||
|
||||
GodotCollisionConfiguration::GodotCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo)
|
||||
: btDefaultCollisionConfiguration(constructionInfo) {
|
||||
|
||||
void *mem = NULL;
|
||||
|
||||
mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::CreateFunc), 16);
|
||||
m_rayWorldCF = new (mem) GodotRayWorldAlgorithm::CreateFunc(world);
|
||||
|
||||
mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::SwappedCreateFunc), 16);
|
||||
m_swappedRayWorldCF = new (mem) GodotRayWorldAlgorithm::SwappedCreateFunc(world);
|
||||
}
|
||||
|
||||
GodotCollisionConfiguration::~GodotCollisionConfiguration() {
|
||||
m_rayWorldCF->~btCollisionAlgorithmCreateFunc();
|
||||
btAlignedFree(m_rayWorldCF);
|
||||
|
||||
m_swappedRayWorldCF->~btCollisionAlgorithmCreateFunc();
|
||||
btAlignedFree(m_swappedRayWorldCF);
|
||||
}
|
||||
|
||||
btCollisionAlgorithmCreateFunc *GodotCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) {
|
||||
|
||||
if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
|
||||
|
||||
// This collision is not supported
|
||||
return m_emptyCreateFunc;
|
||||
} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
|
||||
|
||||
return m_rayWorldCF;
|
||||
} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
|
||||
|
||||
return m_swappedRayWorldCF;
|
||||
} else {
|
||||
|
||||
return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1);
|
||||
}
|
||||
}
|
||||
|
||||
btCollisionAlgorithmCreateFunc *GodotCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) {
|
||||
|
||||
if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
|
||||
|
||||
// This collision is not supported
|
||||
return m_emptyCreateFunc;
|
||||
} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
|
||||
|
||||
return m_rayWorldCF;
|
||||
} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
|
||||
|
||||
return m_swappedRayWorldCF;
|
||||
} else {
|
||||
|
||||
return btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(proxyType0, proxyType1);
|
||||
}
|
||||
}
|
50
modules/bullet/godot_collision_configuration.h
Normal file
50
modules/bullet/godot_collision_configuration.h
Normal file
|
@ -0,0 +1,50 @@
|
|||
/*************************************************************************/
|
||||
/* godot_collision_configuration.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef GODOT_COLLISION_CONFIGURATION_H
|
||||
#define GODOT_COLLISION_CONFIGURATION_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
|
||||
|
||||
class btDiscreteDynamicsWorld;
|
||||
|
||||
class GodotCollisionConfiguration : public btDefaultCollisionConfiguration {
|
||||
btCollisionAlgorithmCreateFunc *m_rayWorldCF;
|
||||
btCollisionAlgorithmCreateFunc *m_swappedRayWorldCF;
|
||||
|
||||
public:
|
||||
GodotCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo = btDefaultCollisionConstructionInfo());
|
||||
virtual ~GodotCollisionConfiguration();
|
||||
|
||||
virtual btCollisionAlgorithmCreateFunc *getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
|
||||
virtual btCollisionAlgorithmCreateFunc *getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
|
||||
};
|
||||
#endif
|
54
modules/bullet/godot_collision_dispatcher.cpp
Normal file
54
modules/bullet/godot_collision_dispatcher.cpp
Normal file
|
@ -0,0 +1,54 @@
|
|||
/*************************************************************************/
|
||||
/* godot_collision_dispatcher.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "godot_collision_dispatcher.h"
|
||||
#include "collision_object_bullet.h"
|
||||
|
||||
const int GodotCollisionDispatcher::CASTED_TYPE_AREA = static_cast<int>(CollisionObjectBullet::TYPE_AREA);
|
||||
|
||||
GodotCollisionDispatcher::GodotCollisionDispatcher(btCollisionConfiguration *collisionConfiguration)
|
||||
: btCollisionDispatcher(collisionConfiguration) {}
|
||||
|
||||
bool GodotCollisionDispatcher::needsCollision(const btCollisionObject *body0, const btCollisionObject *body1) {
|
||||
if (body0->getUserIndex() == CASTED_TYPE_AREA || body1->getUserIndex() == CASTED_TYPE_AREA) {
|
||||
// Avoide area narrow phase
|
||||
return false;
|
||||
}
|
||||
return btCollisionDispatcher::needsCollision(body0, body1);
|
||||
}
|
||||
|
||||
bool GodotCollisionDispatcher::needsResponse(const btCollisionObject *body0, const btCollisionObject *body1) {
|
||||
if (body0->getUserIndex() == CASTED_TYPE_AREA || body1->getUserIndex() == CASTED_TYPE_AREA) {
|
||||
// Avoide area narrow phase
|
||||
return false;
|
||||
}
|
||||
return btCollisionDispatcher::needsResponse(body0, body1);
|
||||
}
|
48
modules/bullet/godot_collision_dispatcher.h
Normal file
48
modules/bullet/godot_collision_dispatcher.h
Normal file
|
@ -0,0 +1,48 @@
|
|||
/*************************************************************************/
|
||||
/* godot_collision_dispatcher.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef GODOT_COLLISION_DISPATCHER_H
|
||||
#define GODOT_COLLISION_DISPATCHER_H
|
||||
|
||||
#include "int_types.h"
|
||||
#include <btBulletDynamicsCommon.h>
|
||||
|
||||
/// This class is required to implement custom collision behaviour in the narrowphase
|
||||
class GodotCollisionDispatcher : public btCollisionDispatcher {
|
||||
private:
|
||||
static const int CASTED_TYPE_AREA;
|
||||
|
||||
public:
|
||||
GodotCollisionDispatcher(btCollisionConfiguration *collisionConfiguration);
|
||||
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1);
|
||||
virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1);
|
||||
};
|
||||
#endif
|
96
modules/bullet/godot_motion_state.h
Normal file
96
modules/bullet/godot_motion_state.h
Normal file
|
@ -0,0 +1,96 @@
|
|||
/*************************************************************************/
|
||||
/* godot_motion_state.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef GODOT_MOTION_STATE_H
|
||||
#define GODOT_MOTION_STATE_H
|
||||
|
||||
#include "LinearMath/btMotionState.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
|
||||
// This clas is responsible to move kinematic actor
|
||||
// and sincronize rendering engine with Bullet
|
||||
/// DOC:
|
||||
/// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/MotionStates#What.27s_a_MotionState.3F
|
||||
class GodotMotionState : public btMotionState {
|
||||
|
||||
/// This data is used to store the new world position for kinematic body
|
||||
btTransform bodyKinematicWorldTransf;
|
||||
/// This data is used to store last world position
|
||||
btTransform bodyCurrentWorldTransform;
|
||||
|
||||
RigidBodyBullet *owner;
|
||||
|
||||
public:
|
||||
GodotMotionState(RigidBodyBullet *p_owner)
|
||||
: bodyKinematicWorldTransf(btMatrix3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), btVector3(0., 0., 0.)),
|
||||
bodyCurrentWorldTransform(btMatrix3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), btVector3(0., 0., 0.)),
|
||||
owner(p_owner) {}
|
||||
|
||||
/// IMPORTANT DON'T USE THIS FUNCTION TO KNOW THE CURRENT BODY TRANSFORM
|
||||
/// This class is used internally by Bullet
|
||||
/// Use GodotMotionState::getCurrentWorldTransform to know current position
|
||||
///
|
||||
/// This function is used by Bullet to get the position of object in the world
|
||||
/// if the body is kinematic Bullet will move the object to this location
|
||||
/// if the body is static Bullet doesn't move at all
|
||||
virtual void getWorldTransform(btTransform &worldTrans) const {
|
||||
worldTrans = bodyKinematicWorldTransf;
|
||||
}
|
||||
|
||||
/// IMPORTANT: to move the body use: moveBody
|
||||
/// IMPORTANT: DON'T CALL THIS FUNCTION, IT IS CALLED BY BULLET TO UPDATE RENDERING ENGINE
|
||||
///
|
||||
/// This function is called each time by Bullet and set the current position of body
|
||||
/// inside the physics world.
|
||||
/// Don't allow Godot rendering scene takes world transform from this object because
|
||||
/// the correct transform is set by Bullet only after the last step when there are sub steps
|
||||
/// This function must update Godot transform rendering scene for this object.
|
||||
virtual void setWorldTransform(const btTransform &worldTrans) {
|
||||
bodyCurrentWorldTransform = worldTrans;
|
||||
|
||||
owner->scratch();
|
||||
}
|
||||
|
||||
public:
|
||||
/// Use this function to move kinematic body
|
||||
/// -- or set initial transfom before body creation.
|
||||
void moveBody(const btTransform &newWorldTransform) {
|
||||
bodyKinematicWorldTransf = newWorldTransform;
|
||||
}
|
||||
|
||||
/// It returns the current body transform from last Bullet update
|
||||
const btTransform &getCurrentWorldTransform() const {
|
||||
return bodyCurrentWorldTransform;
|
||||
}
|
||||
};
|
||||
#endif
|
104
modules/bullet/godot_ray_world_algorithm.cpp
Normal file
104
modules/bullet/godot_ray_world_algorithm.cpp
Normal file
|
@ -0,0 +1,104 @@
|
|||
/*************************************************************************/
|
||||
/* godot_ray_world_algorithm.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "godot_ray_world_algorithm.h"
|
||||
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
|
||||
#include "btRayShape.h"
|
||||
#include "collision_object_bullet.h"
|
||||
|
||||
GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world)
|
||||
: m_world(world) {}
|
||||
|
||||
GodotRayWorldAlgorithm::SwappedCreateFunc::SwappedCreateFunc(const btDiscreteDynamicsWorld *world)
|
||||
: m_world(world) {}
|
||||
|
||||
GodotRayWorldAlgorithm::GodotRayWorldAlgorithm(const btDiscreteDynamicsWorld *world, btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
|
||||
: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
|
||||
m_manifoldPtr(mf),
|
||||
m_ownManifold(false),
|
||||
m_world(world),
|
||||
m_isSwapped(isSwapped) {}
|
||||
|
||||
GodotRayWorldAlgorithm::~GodotRayWorldAlgorithm() {
|
||||
if (m_ownManifold && m_manifoldPtr) {
|
||||
m_dispatcher->releaseManifold(m_manifoldPtr);
|
||||
}
|
||||
}
|
||||
|
||||
void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut) {
|
||||
|
||||
if (!m_manifoldPtr) {
|
||||
if (m_isSwapped) {
|
||||
m_manifoldPtr = m_dispatcher->getNewManifold(body1Wrap->getCollisionObject(), body0Wrap->getCollisionObject());
|
||||
} else {
|
||||
m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
|
||||
}
|
||||
m_ownManifold = true;
|
||||
}
|
||||
m_manifoldPtr->clearManifold();
|
||||
resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
|
||||
const btRayShape *ray_shape;
|
||||
btTransform ray_transform;
|
||||
|
||||
const btCollisionObjectWrapper *other_co_wrapper;
|
||||
|
||||
if (m_isSwapped) {
|
||||
|
||||
ray_shape = static_cast<const btRayShape *>(body1Wrap->getCollisionShape());
|
||||
ray_transform = body1Wrap->getWorldTransform();
|
||||
|
||||
other_co_wrapper = body0Wrap;
|
||||
} else {
|
||||
|
||||
ray_shape = static_cast<const btRayShape *>(body0Wrap->getCollisionShape());
|
||||
ray_transform = body0Wrap->getWorldTransform();
|
||||
|
||||
other_co_wrapper = body1Wrap;
|
||||
}
|
||||
|
||||
btTransform to(ray_transform * ray_shape->getSupportPoint());
|
||||
|
||||
btCollisionWorld::ClosestRayResultCallback btResult(ray_transform.getOrigin(), to.getOrigin());
|
||||
|
||||
m_world->rayTestSingleInternal(ray_transform, to, other_co_wrapper, btResult);
|
||||
|
||||
if (btResult.hasHit()) {
|
||||
btVector3 ray_normal(to.getOrigin() - ray_transform.getOrigin());
|
||||
ray_normal.normalize();
|
||||
ray_normal *= -1;
|
||||
resultOut->addContactPoint(ray_normal, btResult.m_hitPointWorld, ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotRayWorldAlgorithm::calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut) {
|
||||
return 1;
|
||||
}
|
83
modules/bullet/godot_ray_world_algorithm.h
Normal file
83
modules/bullet/godot_ray_world_algorithm.h
Normal file
|
@ -0,0 +1,83 @@
|
|||
/*************************************************************************/
|
||||
/* godot_ray_world_algorithm.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef GODOT_RAY_WORLD_ALGORITHM_H
|
||||
#define GODOT_RAY_WORLD_ALGORITHM_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
|
||||
class btDiscreteDynamicsWorld;
|
||||
|
||||
class GodotRayWorldAlgorithm : public btActivatingCollisionAlgorithm {
|
||||
|
||||
const btDiscreteDynamicsWorld *m_world;
|
||||
btPersistentManifold *m_manifoldPtr;
|
||||
bool m_ownManifold;
|
||||
bool m_isSwapped;
|
||||
|
||||
public:
|
||||
GodotRayWorldAlgorithm(const btDiscreteDynamicsWorld *m_world, btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped);
|
||||
virtual ~GodotRayWorldAlgorithm();
|
||||
|
||||
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut);
|
||||
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut);
|
||||
|
||||
virtual void getAllContactManifolds(btManifoldArray &manifoldArray) {
|
||||
///should we use m_ownManifold to avoid adding duplicates?
|
||||
if (m_manifoldPtr && m_ownManifold)
|
||||
manifoldArray.push_back(m_manifoldPtr);
|
||||
}
|
||||
struct CreateFunc : public btCollisionAlgorithmCreateFunc {
|
||||
|
||||
const btDiscreteDynamicsWorld *m_world;
|
||||
CreateFunc(const btDiscreteDynamicsWorld *world);
|
||||
|
||||
virtual btCollisionAlgorithm *CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) {
|
||||
void *mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(GodotRayWorldAlgorithm));
|
||||
return new (mem) GodotRayWorldAlgorithm(m_world, ci.m_manifold, ci, body0Wrap, body1Wrap, false);
|
||||
}
|
||||
};
|
||||
|
||||
struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc {
|
||||
|
||||
const btDiscreteDynamicsWorld *m_world;
|
||||
SwappedCreateFunc(const btDiscreteDynamicsWorld *world);
|
||||
|
||||
virtual btCollisionAlgorithm *CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) {
|
||||
void *mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(GodotRayWorldAlgorithm));
|
||||
return new (mem) GodotRayWorldAlgorithm(m_world, ci.m_manifold, ci, body0Wrap, body1Wrap, true);
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
#endif // GODOT_RAY_WORLD_ALGORITHM_H
|
291
modules/bullet/godot_result_callbacks.cpp
Normal file
291
modules/bullet/godot_result_callbacks.cpp
Normal file
|
@ -0,0 +1,291 @@
|
|||
/*************************************************************************/
|
||||
/* godot_result_callbacks.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "godot_result_callbacks.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "collision_object_bullet.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
bool GodotFilterCallback::test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask) {
|
||||
return body0_collision_layer & body1_collision_mask || body1_collision_layer & body0_collision_mask;
|
||||
}
|
||||
|
||||
bool GodotFilterCallback::needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const {
|
||||
return GodotFilterCallback::test_collision_filters(proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask, proxy1->m_collisionFilterGroup, proxy1->m_collisionFilterMask);
|
||||
}
|
||||
|
||||
bool GodotClosestRayResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (m_pickRay && gObj->is_ray_pickable()) {
|
||||
return true;
|
||||
} else if (m_exclude->has(gObj->get_self())) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool GodotAllConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (m_exclude->has(gObj->get_self())) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(convexResult.m_hitCollisionObject->getUserPointer());
|
||||
|
||||
PhysicsDirectSpaceState::ShapeResult &result = m_results[count];
|
||||
|
||||
result.shape = convexResult.m_localShapeInfo->m_shapePart;
|
||||
result.rid = gObj->get_self();
|
||||
result.collider_id = gObj->get_instance_id();
|
||||
result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
|
||||
|
||||
++count;
|
||||
return count < m_resultMax;
|
||||
}
|
||||
|
||||
bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (gObj == m_self_object) {
|
||||
return false;
|
||||
} else {
|
||||
if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
|
||||
return false;
|
||||
} else if (m_self_object->has_collision_exception(gObj)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (m_exclude->has(gObj->get_self())) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotClosestConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
|
||||
btScalar res = btCollisionWorld::ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
|
||||
m_shapePart = convexResult.m_localShapeInfo->m_shapePart;
|
||||
return res;
|
||||
}
|
||||
|
||||
bool GodotAllContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (m_exclude->has(gObj->get_self())) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotAllContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
|
||||
|
||||
if (cp.getDistance() <= 0) {
|
||||
|
||||
PhysicsDirectSpaceState::ShapeResult &result = m_results[m_count];
|
||||
// Penetrated
|
||||
|
||||
CollisionObjectBullet *colObj;
|
||||
if (m_self_object == colObj0Wrap->getCollisionObject()) {
|
||||
colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
|
||||
result.shape = cp.m_index1;
|
||||
} else {
|
||||
colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
|
||||
result.shape = cp.m_index0;
|
||||
}
|
||||
|
||||
if (colObj)
|
||||
result.collider_id = colObj->get_instance_id();
|
||||
else
|
||||
result.collider_id = 0;
|
||||
result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
|
||||
result.rid = colObj->get_self();
|
||||
++m_count;
|
||||
}
|
||||
|
||||
return m_count < m_resultMax;
|
||||
}
|
||||
|
||||
bool GodotContactPairContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (m_exclude->has(gObj->get_self())) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotContactPairContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
|
||||
|
||||
if (m_self_object == colObj0Wrap->getCollisionObject()) {
|
||||
B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 0]); // Local contact
|
||||
B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 1]);
|
||||
} else {
|
||||
B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 0]); // Local contact
|
||||
B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 1]);
|
||||
}
|
||||
|
||||
++m_count;
|
||||
|
||||
return m_count < m_resultMax;
|
||||
}
|
||||
|
||||
bool GodotRestInfoContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (m_exclude->has(gObj->get_self())) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
|
||||
|
||||
if (cp.getDistance() <= m_min_distance) {
|
||||
m_min_distance = cp.getDistance();
|
||||
|
||||
CollisionObjectBullet *colObj;
|
||||
if (m_self_object == colObj0Wrap->getCollisionObject()) {
|
||||
colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
|
||||
m_result->shape = cp.m_index1;
|
||||
B_TO_G(cp.getPositionWorldOnB(), m_result->point);
|
||||
m_rest_info_bt_point = cp.getPositionWorldOnB();
|
||||
m_rest_info_collision_object = colObj1Wrap->getCollisionObject();
|
||||
} else {
|
||||
colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
|
||||
m_result->shape = cp.m_index0;
|
||||
B_TO_G(cp.m_normalWorldOnB * -1, m_result->normal);
|
||||
m_rest_info_bt_point = cp.getPositionWorldOnA();
|
||||
m_rest_info_collision_object = colObj0Wrap->getCollisionObject();
|
||||
}
|
||||
|
||||
if (colObj)
|
||||
m_result->collider_id = colObj->get_instance_id();
|
||||
else
|
||||
m_result->collider_id = 0;
|
||||
m_result->rid = colObj->get_self();
|
||||
|
||||
m_collided = true;
|
||||
}
|
||||
|
||||
return cp.getDistance();
|
||||
}
|
||||
|
||||
bool GodotRecoverAndClosestContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
|
||||
const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
|
||||
if (needs) {
|
||||
btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
|
||||
CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
|
||||
if (gObj == m_self_object) {
|
||||
return false;
|
||||
} else {
|
||||
if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
|
||||
return false;
|
||||
} else if (m_self_object->has_collision_exception(gObj)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar GodotRecoverAndClosestContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
|
||||
|
||||
if (cp.getDistance() < -MAX_PENETRATION_DEPTH) {
|
||||
if (m_most_penetrated_distance > cp.getDistance()) {
|
||||
m_most_penetrated_distance = cp.getDistance();
|
||||
|
||||
// take other object
|
||||
btScalar sign(1);
|
||||
if (m_self_object == colObj0Wrap->getCollisionObject()->getUserPointer()) {
|
||||
m_pointCollisionObject = colObj1Wrap->getCollisionObject();
|
||||
m_other_compound_shape_index = cp.m_index1;
|
||||
} else {
|
||||
m_pointCollisionObject = colObj0Wrap->getCollisionObject();
|
||||
sign = -1;
|
||||
m_other_compound_shape_index = cp.m_index0;
|
||||
}
|
||||
|
||||
m_pointNormalWorld = cp.m_normalWorldOnB * sign;
|
||||
m_pointWorld = cp.getPositionWorldOnB();
|
||||
m_penetration_distance = cp.getDistance();
|
||||
|
||||
m_recover_penetration -= cp.m_normalWorldOnB * sign * (cp.getDistance() + MAX_PENETRATION_DEPTH);
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
189
modules/bullet/godot_result_callbacks.h
Normal file
189
modules/bullet/godot_result_callbacks.h
Normal file
|
@ -0,0 +1,189 @@
|
|||
/*************************************************************************/
|
||||
/* godot_result_callbacks.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef GODOT_RESULT_CALLBACKS_H
|
||||
#define GODOT_RESULT_CALLBACKS_H
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "servers/physics_server.h"
|
||||
|
||||
#define MAX_PENETRATION_DEPTH 0.005
|
||||
|
||||
class RigidBodyBullet;
|
||||
|
||||
/// This class is required to implement custom collision behaviour in the broadphase
|
||||
struct GodotFilterCallback : public btOverlapFilterCallback {
|
||||
static bool test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask);
|
||||
|
||||
// return true when pairs need collision
|
||||
virtual bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const;
|
||||
};
|
||||
|
||||
/// It performs an additional check allow exclusions.
|
||||
struct GodotClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback {
|
||||
const Set<RID> *m_exclude;
|
||||
bool m_pickRay;
|
||||
|
||||
public:
|
||||
GodotClosestRayResultCallback(const btVector3 &rayFromWorld, const btVector3 &rayToWorld, const Set<RID> *p_exclude)
|
||||
: btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), m_exclude(p_exclude), m_pickRay(false) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
};
|
||||
|
||||
// store all colliding object
|
||||
struct GodotAllConvexResultCallback : public btCollisionWorld::ConvexResultCallback {
|
||||
public:
|
||||
PhysicsDirectSpaceState::ShapeResult *m_results;
|
||||
int m_resultMax;
|
||||
int count;
|
||||
const Set<RID> *m_exclude;
|
||||
|
||||
GodotAllConvexResultCallback(PhysicsDirectSpaceState::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude)
|
||||
: m_results(p_results), m_exclude(p_exclude), m_resultMax(p_resultMax), count(0) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
|
||||
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace);
|
||||
};
|
||||
|
||||
struct GodotKinClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
|
||||
public:
|
||||
const RigidBodyBullet *m_self_object;
|
||||
const bool m_ignore_areas;
|
||||
|
||||
GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_ignore_areas)
|
||||
: btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld), m_self_object(p_self_object), m_ignore_areas(p_ignore_areas) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
};
|
||||
|
||||
struct GodotClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
|
||||
public:
|
||||
const Set<RID> *m_exclude;
|
||||
int m_shapePart;
|
||||
|
||||
GodotClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const Set<RID> *p_exclude)
|
||||
: btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld), m_exclude(p_exclude) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
|
||||
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace);
|
||||
};
|
||||
|
||||
struct GodotAllContactResultCallback : public btCollisionWorld::ContactResultCallback {
|
||||
public:
|
||||
const btCollisionObject *m_self_object;
|
||||
PhysicsDirectSpaceState::ShapeResult *m_results;
|
||||
int m_resultMax;
|
||||
int m_count;
|
||||
const Set<RID> *m_exclude;
|
||||
|
||||
GodotAllContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude)
|
||||
: m_self_object(p_self_object), m_results(p_results), m_exclude(p_exclude), m_resultMax(p_resultMax), m_count(0) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
|
||||
virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
|
||||
};
|
||||
|
||||
/// Returns the list of contacts pairs in this order: Local contact, other body contact
|
||||
struct GodotContactPairContactResultCallback : public btCollisionWorld::ContactResultCallback {
|
||||
public:
|
||||
const btCollisionObject *m_self_object;
|
||||
Vector3 *m_results;
|
||||
int m_resultMax;
|
||||
int m_count;
|
||||
const Set<RID> *m_exclude;
|
||||
|
||||
GodotContactPairContactResultCallback(btCollisionObject *p_self_object, Vector3 *p_results, int p_resultMax, const Set<RID> *p_exclude)
|
||||
: m_self_object(p_self_object), m_results(p_results), m_exclude(p_exclude), m_resultMax(p_resultMax), m_count(0) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
|
||||
virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
|
||||
};
|
||||
|
||||
struct GodotRestInfoContactResultCallback : public btCollisionWorld::ContactResultCallback {
|
||||
public:
|
||||
const btCollisionObject *m_self_object;
|
||||
PhysicsDirectSpaceState::ShapeRestInfo *m_result;
|
||||
bool m_collided;
|
||||
real_t m_min_distance;
|
||||
const btCollisionObject *m_rest_info_collision_object;
|
||||
btVector3 m_rest_info_bt_point;
|
||||
const Set<RID> *m_exclude;
|
||||
|
||||
GodotRestInfoContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState::ShapeRestInfo *p_result, const Set<RID> *p_exclude)
|
||||
: m_self_object(p_self_object), m_result(p_result), m_exclude(p_exclude), m_collided(false), m_min_distance(0) {}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
|
||||
virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
|
||||
};
|
||||
|
||||
struct GodotRecoverAndClosestContactResultCallback : public btCollisionWorld::ContactResultCallback {
|
||||
public:
|
||||
btVector3 m_pointNormalWorld;
|
||||
btVector3 m_pointWorld;
|
||||
btScalar m_penetration_distance;
|
||||
int m_other_compound_shape_index;
|
||||
const btCollisionObject *m_pointCollisionObject;
|
||||
|
||||
const RigidBodyBullet *m_self_object;
|
||||
bool m_ignore_areas;
|
||||
|
||||
btScalar m_most_penetrated_distance;
|
||||
btVector3 m_recover_penetration;
|
||||
|
||||
GodotRecoverAndClosestContactResultCallback()
|
||||
: m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(NULL), m_ignore_areas(true), m_most_penetrated_distance(1e20), m_recover_penetration(0, 0, 0) {}
|
||||
|
||||
GodotRecoverAndClosestContactResultCallback(const RigidBodyBullet *p_self_object, bool p_ignore_areas)
|
||||
: m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(p_self_object), m_ignore_areas(p_ignore_areas), m_most_penetrated_distance(9999999999), m_recover_penetration(0, 0, 0) {}
|
||||
|
||||
void reset() {
|
||||
m_pointCollisionObject = NULL;
|
||||
m_most_penetrated_distance = 1e20;
|
||||
m_recover_penetration.setZero();
|
||||
}
|
||||
|
||||
bool hasHit() {
|
||||
return m_pointCollisionObject;
|
||||
}
|
||||
|
||||
virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
|
||||
|
||||
virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
|
||||
};
|
||||
|
||||
#endif // GODOT_RESULT_CALLBACKS_H
|
163
modules/bullet/hinge_joint_bullet.cpp
Normal file
163
modules/bullet/hinge_joint_bullet.cpp
Normal file
|
@ -0,0 +1,163 @@
|
|||
/*************************************************************************/
|
||||
/* hinge_joint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "hinge_joint_bullet.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB)
|
||||
: JointBullet() {
|
||||
btTransform btFrameA;
|
||||
G_TO_B(frameA, btFrameA);
|
||||
|
||||
if (rbB) {
|
||||
btTransform btFrameB;
|
||||
G_TO_B(frameB, btFrameB);
|
||||
|
||||
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
|
||||
} else {
|
||||
|
||||
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btFrameA));
|
||||
}
|
||||
|
||||
setup(hingeConstraint);
|
||||
}
|
||||
|
||||
HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB)
|
||||
: JointBullet() {
|
||||
|
||||
btVector3 btPivotA;
|
||||
btVector3 btAxisA;
|
||||
G_TO_B(pivotInA, btPivotA);
|
||||
G_TO_B(axisInA, btAxisA);
|
||||
|
||||
if (rbB) {
|
||||
btVector3 btPivotB;
|
||||
btVector3 btAxisB;
|
||||
G_TO_B(pivotInB, btPivotB);
|
||||
G_TO_B(axisInB, btAxisB);
|
||||
|
||||
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btPivotA, btPivotB, btAxisA, btAxisB));
|
||||
} else {
|
||||
|
||||
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btPivotA, btAxisA));
|
||||
}
|
||||
|
||||
setup(hingeConstraint);
|
||||
}
|
||||
|
||||
real_t HingeJointBullet::get_hinge_angle() {
|
||||
return hingeConstraint->getHingeAngle();
|
||||
}
|
||||
|
||||
void HingeJointBullet::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::HINGE_JOINT_BIAS:
|
||||
if (0 < p_value) {
|
||||
print_line("The Bullet Hinge Joint doesn't support bias, So it's always 0");
|
||||
}
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_UPPER:
|
||||
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), p_value, hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_LOWER:
|
||||
hingeConstraint->setLimit(p_value, hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_BIAS:
|
||||
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), p_value, hingeConstraint->getLimitRelaxationFactor());
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS:
|
||||
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), p_value, hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION:
|
||||
hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), p_value);
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
|
||||
hingeConstraint->setMotorTargetVelocity(p_value);
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE:
|
||||
hingeConstraint->setMaxMotorImpulse(p_value);
|
||||
break;
|
||||
default:
|
||||
WARN_PRINTS("The Bullet Hinge Joint doesn't support this parameter: " + itos(p_param) + ", value: " + itos(p_value));
|
||||
}
|
||||
}
|
||||
|
||||
real_t HingeJointBullet::get_param(PhysicsServer::HingeJointParam p_param) const {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::HINGE_JOINT_BIAS:
|
||||
return 0;
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_UPPER:
|
||||
return hingeConstraint->getUpperLimit();
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_LOWER:
|
||||
return hingeConstraint->getLowerLimit();
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_BIAS:
|
||||
return hingeConstraint->getLimitBiasFactor();
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS:
|
||||
return hingeConstraint->getLimitSoftness();
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION:
|
||||
return hingeConstraint->getLimitRelaxationFactor();
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
|
||||
return hingeConstraint->getMotorTargetVelocity();
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE:
|
||||
return hingeConstraint->getMaxMotorImpulse();
|
||||
default:
|
||||
WARN_PRINTS("The Bullet Hinge Joint doesn't support this parameter: " + itos(p_param));
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void HingeJointBullet::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) {
|
||||
switch (p_flag) {
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT:
|
||||
if (!p_value) {
|
||||
hingeConstraint->setLimit(-Math_PI, Math_PI);
|
||||
}
|
||||
break;
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:
|
||||
hingeConstraint->enableMotor(p_value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool HingeJointBullet::get_flag(PhysicsServer::HingeJointFlag p_flag) const {
|
||||
switch (p_flag) {
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT:
|
||||
return true;
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:
|
||||
return hingeConstraint->getEnableAngularMotor();
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
54
modules/bullet/hinge_joint_bullet.h
Normal file
54
modules/bullet/hinge_joint_bullet.h
Normal file
|
@ -0,0 +1,54 @@
|
|||
/*************************************************************************/
|
||||
/* hinge_joint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef HINGE_JOINT_BULLET_H
|
||||
#define HINGE_JOINT_BULLET_H
|
||||
|
||||
#include "joint_bullet.h"
|
||||
|
||||
class HingeJointBullet : public JointBullet {
|
||||
class btHingeConstraint *hingeConstraint;
|
||||
|
||||
public:
|
||||
HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB);
|
||||
HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
|
||||
|
||||
real_t get_hinge_angle();
|
||||
|
||||
void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value);
|
||||
real_t get_param(PhysicsServer::HingeJointParam p_param) const;
|
||||
|
||||
void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
|
||||
bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
|
||||
};
|
||||
#endif
|
38
modules/bullet/joint_bullet.cpp
Normal file
38
modules/bullet/joint_bullet.cpp
Normal file
|
@ -0,0 +1,38 @@
|
|||
/*************************************************************************/
|
||||
/* joint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "joint_bullet.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
JointBullet::JointBullet()
|
||||
: ConstraintBullet() {}
|
||||
|
||||
JointBullet::~JointBullet() {}
|
49
modules/bullet/joint_bullet.h
Normal file
49
modules/bullet/joint_bullet.h
Normal file
|
@ -0,0 +1,49 @@
|
|||
/*************************************************************************/
|
||||
/* joint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef JOINT_BULLET_H
|
||||
#define JOINT_BULLET_H
|
||||
|
||||
#include "constraint_bullet.h"
|
||||
#include "servers/physics_server.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
class btTypedConstraint;
|
||||
|
||||
class JointBullet : public ConstraintBullet {
|
||||
|
||||
public:
|
||||
JointBullet();
|
||||
virtual ~JointBullet();
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const = 0;
|
||||
};
|
||||
#endif
|
112
modules/bullet/pin_joint_bullet.cpp
Normal file
112
modules/bullet/pin_joint_bullet.cpp
Normal file
|
@ -0,0 +1,112 @@
|
|||
/*************************************************************************/
|
||||
/* pin_joint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "pin_joint_bullet.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
PinJointBullet::PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a, RigidBodyBullet *p_body_b, const Vector3 &p_pos_b)
|
||||
: JointBullet() {
|
||||
if (p_body_b) {
|
||||
|
||||
btVector3 btPivotA;
|
||||
btVector3 btPivotB;
|
||||
G_TO_B(p_pos_a, btPivotA);
|
||||
G_TO_B(p_pos_b, btPivotB);
|
||||
p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(),
|
||||
*p_body_b->get_bt_rigid_body(),
|
||||
btPivotA,
|
||||
btPivotB));
|
||||
} else {
|
||||
btVector3 btPivotA;
|
||||
G_TO_B(p_pos_a, btPivotA);
|
||||
p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(), btPivotA));
|
||||
}
|
||||
|
||||
setup(p2pConstraint);
|
||||
}
|
||||
|
||||
PinJointBullet::~PinJointBullet() {}
|
||||
|
||||
void PinJointBullet::set_param(PhysicsServer::PinJointParam p_param, real_t p_value) {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::PIN_JOINT_BIAS:
|
||||
p2pConstraint->m_setting.m_tau = p_value;
|
||||
break;
|
||||
case PhysicsServer::PIN_JOINT_DAMPING:
|
||||
p2pConstraint->m_setting.m_damping = p_value;
|
||||
break;
|
||||
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP:
|
||||
p2pConstraint->m_setting.m_impulseClamp = p_value;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
real_t PinJointBullet::get_param(PhysicsServer::PinJointParam p_param) const {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::PIN_JOINT_BIAS:
|
||||
return p2pConstraint->m_setting.m_tau;
|
||||
case PhysicsServer::PIN_JOINT_DAMPING:
|
||||
return p2pConstraint->m_setting.m_damping;
|
||||
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP:
|
||||
return p2pConstraint->m_setting.m_impulseClamp;
|
||||
default:
|
||||
WARN_PRINTS("This get parameter is not supported");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void PinJointBullet::setPivotInA(const Vector3 &p_pos) {
|
||||
btVector3 btVec;
|
||||
G_TO_B(p_pos, btVec);
|
||||
p2pConstraint->setPivotA(btVec);
|
||||
}
|
||||
|
||||
void PinJointBullet::setPivotInB(const Vector3 &p_pos) {
|
||||
btVector3 btVec;
|
||||
G_TO_B(p_pos, btVec);
|
||||
p2pConstraint->setPivotB(btVec);
|
||||
}
|
||||
|
||||
Vector3 PinJointBullet::getPivotInA() {
|
||||
btVector3 vec = p2pConstraint->getPivotInA();
|
||||
Vector3 gVec;
|
||||
B_TO_G(vec, gVec);
|
||||
return gVec;
|
||||
}
|
||||
|
||||
Vector3 PinJointBullet::getPivotInB() {
|
||||
btVector3 vec = p2pConstraint->getPivotInB();
|
||||
Vector3 gVec;
|
||||
B_TO_G(vec, gVec);
|
||||
return gVec;
|
||||
}
|
57
modules/bullet/pin_joint_bullet.h
Normal file
57
modules/bullet/pin_joint_bullet.h
Normal file
|
@ -0,0 +1,57 @@
|
|||
/*************************************************************************/
|
||||
/* pin_joint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef PIN_JOINT_BULLET_H
|
||||
#define PIN_JOINT_BULLET_H
|
||||
|
||||
#include "joint_bullet.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
|
||||
class PinJointBullet : public JointBullet {
|
||||
class btPoint2PointConstraint *p2pConstraint;
|
||||
|
||||
public:
|
||||
PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a, RigidBodyBullet *p_body_b, const Vector3 &p_pos_b);
|
||||
~PinJointBullet();
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
|
||||
|
||||
void set_param(PhysicsServer::PinJointParam p_param, real_t p_value);
|
||||
real_t get_param(PhysicsServer::PinJointParam p_param) const;
|
||||
|
||||
void setPivotInA(const Vector3 &p_pos);
|
||||
void setPivotInB(const Vector3 &p_pos);
|
||||
|
||||
Vector3 getPivotInA();
|
||||
Vector3 getPivotInB();
|
||||
};
|
||||
#endif
|
47
modules/bullet/register_types.cpp
Normal file
47
modules/bullet/register_types.cpp
Normal file
|
@ -0,0 +1,47 @@
|
|||
/*************************************************************************/
|
||||
/* register_types.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "register_types.h"
|
||||
#include "bullet_physics_server.h"
|
||||
#include "class_db.h"
|
||||
|
||||
PhysicsServer *_createBulletPhysicsCallback() {
|
||||
return memnew(BulletPhysicsServer);
|
||||
}
|
||||
|
||||
void register_bullet_types() {
|
||||
|
||||
PhysicsServerManager::register_server("Bullet", &_createBulletPhysicsCallback);
|
||||
PhysicsServerManager::set_default_server("Bullet", 1);
|
||||
}
|
||||
|
||||
void unregister_bullet_types() {
|
||||
}
|
37
modules/bullet/register_types.h
Normal file
37
modules/bullet/register_types.h
Normal file
|
@ -0,0 +1,37 @@
|
|||
/*************************************************************************/
|
||||
/* register_types.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef REGISTER_BULLET_TYPES_H
|
||||
#define REGISTER_BULLET_TYPES_H
|
||||
|
||||
void register_bullet_types();
|
||||
void unregister_bullet_types();
|
||||
#endif
|
50
modules/bullet/rid_bullet.h
Normal file
50
modules/bullet/rid_bullet.h
Normal file
|
@ -0,0 +1,50 @@
|
|||
/*************************************************************************/
|
||||
/* rid_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef RID_BULLET_H
|
||||
#define RID_BULLET_H
|
||||
|
||||
#include "core/rid.h"
|
||||
|
||||
class BulletPhysicsServer;
|
||||
|
||||
class RIDBullet : public RID_Data {
|
||||
RID self;
|
||||
BulletPhysicsServer *physicsServer;
|
||||
|
||||
public:
|
||||
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
|
||||
_FORCE_INLINE_ RID get_self() const { return self; }
|
||||
|
||||
_FORCE_INLINE_ void _set_physics_server(BulletPhysicsServer *p_physicsServer) { physicsServer = p_physicsServer; }
|
||||
_FORCE_INLINE_ BulletPhysicsServer *get_physics_server() const { return physicsServer; }
|
||||
};
|
||||
#endif
|
1009
modules/bullet/rigid_body_bullet.cpp
Normal file
1009
modules/bullet/rigid_body_bullet.cpp
Normal file
File diff suppressed because it is too large
Load diff
304
modules/bullet/rigid_body_bullet.h
Normal file
304
modules/bullet/rigid_body_bullet.h
Normal file
|
@ -0,0 +1,304 @@
|
|||
/*************************************************************************/
|
||||
/* body_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BODYBULLET_H
|
||||
#define BODYBULLET_H
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "collision_object_bullet.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
class AreaBullet;
|
||||
class SpaceBullet;
|
||||
class btRigidBody;
|
||||
class GodotMotionState;
|
||||
class BulletPhysicsDirectBodyState;
|
||||
|
||||
/// This class could be used in multi thread with few changes but currently
|
||||
/// is setted to be only in one single thread.
|
||||
///
|
||||
/// In the system there is only one object at a time that manage all bodies and is
|
||||
/// created by BulletPhysicsServer and is held by the "singleton" variable of this class
|
||||
/// Each time something require it, the body must be setted again.
|
||||
class BulletPhysicsDirectBodyState : public PhysicsDirectBodyState {
|
||||
GDCLASS(BulletPhysicsDirectBodyState, PhysicsDirectBodyState)
|
||||
|
||||
static BulletPhysicsDirectBodyState *singleton;
|
||||
|
||||
public:
|
||||
/// This class avoid the creation of more object of this class
|
||||
static void initSingleton() {
|
||||
if (!singleton) {
|
||||
singleton = memnew(BulletPhysicsDirectBodyState);
|
||||
}
|
||||
}
|
||||
|
||||
static void destroySingleton() {
|
||||
memdelete(singleton);
|
||||
singleton = NULL;
|
||||
}
|
||||
|
||||
static void singleton_setDeltaTime(real_t p_deltaTime) {
|
||||
singleton->deltaTime = p_deltaTime;
|
||||
}
|
||||
|
||||
static BulletPhysicsDirectBodyState *get_singleton(RigidBodyBullet *p_body) {
|
||||
singleton->body = p_body;
|
||||
return singleton;
|
||||
}
|
||||
|
||||
public:
|
||||
RigidBodyBullet *body;
|
||||
real_t deltaTime;
|
||||
|
||||
private:
|
||||
BulletPhysicsDirectBodyState() {}
|
||||
|
||||
public:
|
||||
virtual Vector3 get_total_gravity() const;
|
||||
virtual float get_total_angular_damp() const;
|
||||
virtual float get_total_linear_damp() const;
|
||||
|
||||
virtual Vector3 get_center_of_mass() const;
|
||||
virtual Basis get_principal_inertia_axes() const;
|
||||
// get the mass
|
||||
virtual float get_inverse_mass() const;
|
||||
// get density of this body space
|
||||
virtual Vector3 get_inverse_inertia() const;
|
||||
// get density of this body space
|
||||
virtual Basis get_inverse_inertia_tensor() const;
|
||||
|
||||
virtual void set_linear_velocity(const Vector3 &p_velocity);
|
||||
virtual Vector3 get_linear_velocity() const;
|
||||
|
||||
virtual void set_angular_velocity(const Vector3 &p_velocity);
|
||||
virtual Vector3 get_angular_velocity() const;
|
||||
|
||||
virtual void set_transform(const Transform &p_transform);
|
||||
virtual Transform get_transform() const;
|
||||
|
||||
virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos);
|
||||
virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j);
|
||||
virtual void apply_torque_impulse(const Vector3 &p_j);
|
||||
|
||||
virtual void set_sleep_state(bool p_enable);
|
||||
virtual bool is_sleeping() const;
|
||||
|
||||
virtual int get_contact_count() const;
|
||||
|
||||
virtual Vector3 get_contact_local_position(int p_contact_idx) const;
|
||||
virtual Vector3 get_contact_local_normal(int p_contact_idx) const;
|
||||
virtual int get_contact_local_shape(int p_contact_idx) const;
|
||||
|
||||
virtual RID get_contact_collider(int p_contact_idx) const;
|
||||
virtual Vector3 get_contact_collider_position(int p_contact_idx) const;
|
||||
virtual ObjectID get_contact_collider_id(int p_contact_idx) const;
|
||||
virtual int get_contact_collider_shape(int p_contact_idx) const;
|
||||
virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const;
|
||||
|
||||
virtual real_t get_step() const { return deltaTime; }
|
||||
virtual void integrate_forces() {
|
||||
// Skip the execution of this function
|
||||
}
|
||||
|
||||
virtual PhysicsDirectSpaceState *get_space_state();
|
||||
};
|
||||
|
||||
class RigidBodyBullet : public RigidCollisionObjectBullet {
|
||||
|
||||
public:
|
||||
struct CollisionData {
|
||||
RigidBodyBullet *otherObject;
|
||||
int other_object_shape;
|
||||
int local_shape;
|
||||
Vector3 hitLocalLocation;
|
||||
Vector3 hitWorldLocation;
|
||||
Vector3 hitNormal;
|
||||
};
|
||||
|
||||
struct ForceIntegrationCallback {
|
||||
ObjectID id;
|
||||
StringName method;
|
||||
Variant udata;
|
||||
};
|
||||
|
||||
/// Used to hold shapes
|
||||
struct KinematicShape {
|
||||
class btConvexShape *shape;
|
||||
btTransform transform;
|
||||
|
||||
KinematicShape()
|
||||
: shape(NULL) {}
|
||||
const bool is_active() const { return shape; }
|
||||
};
|
||||
|
||||
struct KinematicUtilities {
|
||||
RigidBodyBullet *m_owner;
|
||||
btScalar m_margin;
|
||||
btManifoldArray m_manifoldArray; ///keep track of the contact manifolds
|
||||
class btPairCachingGhostObject *m_ghostObject;
|
||||
Vector<KinematicShape> m_shapes;
|
||||
|
||||
KinematicUtilities(RigidBodyBullet *p_owner);
|
||||
~KinematicUtilities();
|
||||
|
||||
/// Used to set the default shape to ghost
|
||||
void resetDefShape();
|
||||
void copyAllOwnerShapes();
|
||||
|
||||
private:
|
||||
void just_delete_shapes(int new_size);
|
||||
};
|
||||
|
||||
private:
|
||||
friend class BulletPhysicsDirectBodyState;
|
||||
|
||||
// This is required only for Kinematic movement
|
||||
KinematicUtilities *kinematic_utilities;
|
||||
|
||||
PhysicsServer::BodyMode mode;
|
||||
PhysicsServer::BodyAxisLock axis_lock;
|
||||
GodotMotionState *godotMotionState;
|
||||
btRigidBody *btBody;
|
||||
real_t mass;
|
||||
real_t gravity_scale;
|
||||
real_t linearDamp;
|
||||
real_t angularDamp;
|
||||
bool can_sleep;
|
||||
|
||||
Vector<CollisionData> collisions;
|
||||
// these parameters are used to avoid vector resize
|
||||
int maxCollisionsDetection;
|
||||
int collisionsCount;
|
||||
|
||||
Vector<AreaBullet *> areasWhereIam;
|
||||
// these parameters are used to avoid vector resize
|
||||
int maxAreasWhereIam;
|
||||
int areaWhereIamCount;
|
||||
// Used to know if the area is used as gravity point
|
||||
int countGravityPointSpaces;
|
||||
bool isScratchedSpaceOverrideModificator;
|
||||
|
||||
bool isTransformChanged;
|
||||
|
||||
ForceIntegrationCallback *force_integration_callback;
|
||||
|
||||
public:
|
||||
RigidBodyBullet();
|
||||
~RigidBodyBullet();
|
||||
|
||||
void init_kinematic_utilities();
|
||||
void destroy_kinematic_utilities();
|
||||
_FORCE_INLINE_ class KinematicUtilities *get_kinematic_utilities() const { return kinematic_utilities; }
|
||||
|
||||
_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
|
||||
|
||||
virtual void reload_body();
|
||||
virtual void set_space(SpaceBullet *p_space);
|
||||
|
||||
virtual void dispatch_callbacks();
|
||||
void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
|
||||
void scratch();
|
||||
void scratch_space_override_modificator();
|
||||
|
||||
virtual void on_collision_filters_change();
|
||||
virtual void on_collision_checker_start();
|
||||
void set_max_collisions_detection(int p_maxCollisionsDetection) {
|
||||
maxCollisionsDetection = p_maxCollisionsDetection;
|
||||
collisions.resize(p_maxCollisionsDetection);
|
||||
collisionsCount = 0;
|
||||
}
|
||||
int get_max_collisions_detection() {
|
||||
return maxCollisionsDetection;
|
||||
}
|
||||
|
||||
bool can_add_collision() { return collisionsCount < maxCollisionsDetection; }
|
||||
bool add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, int p_other_shape_index, int p_local_shape_index);
|
||||
|
||||
void assert_no_constraints();
|
||||
|
||||
void set_activation_state(bool p_active);
|
||||
bool is_active() const;
|
||||
|
||||
void set_param(PhysicsServer::BodyParameter p_param, real_t);
|
||||
real_t get_param(PhysicsServer::BodyParameter p_param) const;
|
||||
|
||||
void set_mode(PhysicsServer::BodyMode p_mode);
|
||||
PhysicsServer::BodyMode get_mode() const;
|
||||
|
||||
void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant);
|
||||
Variant get_state(PhysicsServer::BodyState p_state) const;
|
||||
|
||||
void apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse);
|
||||
void apply_central_impulse(const Vector3 &p_force);
|
||||
void apply_torque_impulse(const Vector3 &p_impulse);
|
||||
|
||||
void apply_force(const Vector3 &p_force, const Vector3 &p_pos);
|
||||
void apply_central_force(const Vector3 &p_force);
|
||||
void apply_torque(const Vector3 &p_force);
|
||||
|
||||
void set_applied_force(const Vector3 &p_force);
|
||||
Vector3 get_applied_force() const;
|
||||
void set_applied_torque(const Vector3 &p_torque);
|
||||
Vector3 get_applied_torque() const;
|
||||
|
||||
void set_axis_lock(PhysicsServer::BodyAxisLock p_lock);
|
||||
PhysicsServer::BodyAxisLock get_axis_lock() const;
|
||||
|
||||
/// Doc:
|
||||
/// http://www.bulletphysics.org/mediawiki-1.5.8/index.php?title=Anti_tunneling_by_Motion_Clamping
|
||||
void set_continuous_collision_detection(bool p_enable);
|
||||
bool is_continuous_collision_detection_enabled() const;
|
||||
|
||||
void set_linear_velocity(const Vector3 &p_velocity);
|
||||
Vector3 get_linear_velocity() const;
|
||||
|
||||
void set_angular_velocity(const Vector3 &p_velocity);
|
||||
Vector3 get_angular_velocity() const;
|
||||
|
||||
virtual void set_transform__bullet(const btTransform &p_global_transform);
|
||||
virtual const btTransform &get_transform__bullet() const;
|
||||
|
||||
virtual void on_shapes_changed();
|
||||
|
||||
virtual void on_enter_area(AreaBullet *p_area);
|
||||
virtual void on_exit_area(AreaBullet *p_area);
|
||||
void reload_space_override_modificator();
|
||||
|
||||
/// Kinematic
|
||||
void reload_kinematic_shapes();
|
||||
|
||||
private:
|
||||
void _internal_set_mass(real_t p_mass);
|
||||
};
|
||||
|
||||
#endif
|
435
modules/bullet/shape_bullet.cpp
Normal file
435
modules/bullet/shape_bullet.cpp
Normal file
|
@ -0,0 +1,435 @@
|
|||
/*************************************************************************/
|
||||
/* shape_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "shape_bullet.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
|
||||
#include "btBulletCollisionCommon.h"
|
||||
#include "btRayShape.h"
|
||||
#include "bullet_physics_server.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "shape_owner_bullet.h"
|
||||
|
||||
ShapeBullet::ShapeBullet() {}
|
||||
|
||||
ShapeBullet::~ShapeBullet() {}
|
||||
|
||||
btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
|
||||
p_btShape->setUserPointer(const_cast<ShapeBullet *>(this));
|
||||
return p_btShape;
|
||||
}
|
||||
|
||||
void ShapeBullet::notifyShapeChanged() {
|
||||
for (Map<ShapeOwnerBullet *, int>::Element *E = owners.front(); E; E = E->next()) {
|
||||
static_cast<ShapeOwnerBullet *>(E->key())->on_shape_changed(this);
|
||||
}
|
||||
}
|
||||
|
||||
void ShapeBullet::add_owner(ShapeOwnerBullet *p_owner) {
|
||||
Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
|
||||
if (E) {
|
||||
E->get()++;
|
||||
} else {
|
||||
owners[p_owner] = 1; // add new owner
|
||||
}
|
||||
}
|
||||
|
||||
void ShapeBullet::remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody) {
|
||||
Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
|
||||
ERR_FAIL_COND(!E);
|
||||
E->get()--;
|
||||
if (p_permanentlyFromThisBody || 0 >= E->get()) {
|
||||
owners.erase(E);
|
||||
}
|
||||
}
|
||||
|
||||
bool ShapeBullet::is_owner(ShapeOwnerBullet *p_owner) const {
|
||||
|
||||
return owners.has(p_owner);
|
||||
}
|
||||
|
||||
const Map<ShapeOwnerBullet *, int> &ShapeBullet::get_owners() const {
|
||||
return owners;
|
||||
}
|
||||
|
||||
btEmptyShape *ShapeBullet::create_shape_empty() {
|
||||
return bulletnew(btEmptyShape);
|
||||
}
|
||||
|
||||
btStaticPlaneShape *ShapeBullet::create_shape_plane(const btVector3 &planeNormal, btScalar planeConstant) {
|
||||
return bulletnew(btStaticPlaneShape(planeNormal, planeConstant));
|
||||
}
|
||||
|
||||
btSphereShape *ShapeBullet::create_shape_sphere(btScalar radius) {
|
||||
return bulletnew(btSphereShape(radius));
|
||||
}
|
||||
|
||||
btBoxShape *ShapeBullet::create_shape_box(const btVector3 &boxHalfExtents) {
|
||||
return bulletnew(btBoxShape(boxHalfExtents));
|
||||
}
|
||||
|
||||
btCapsuleShapeZ *ShapeBullet::create_shape_capsule(btScalar radius, btScalar height) {
|
||||
return bulletnew(btCapsuleShapeZ(radius, height));
|
||||
}
|
||||
|
||||
btConvexPointCloudShape *ShapeBullet::create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling) {
|
||||
return bulletnew(btConvexPointCloudShape(&p_vertices[0], p_vertices.size(), p_local_scaling));
|
||||
}
|
||||
|
||||
btScaledBvhTriangleMeshShape *ShapeBullet::create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling) {
|
||||
if (p_mesh_shape) {
|
||||
return bulletnew(btScaledBvhTriangleMeshShape(p_mesh_shape, p_local_scaling));
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size) {
|
||||
const btScalar ignoredHeightScale(1);
|
||||
const btScalar fieldHeight(500); // Meters
|
||||
const int YAxis = 1; // 0=X, 1=Y, 2=Z
|
||||
const bool flipQuadEdges = false;
|
||||
const void *heightsPtr = p_heights.read().ptr();
|
||||
|
||||
return bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, -fieldHeight, fieldHeight, YAxis, PHY_FLOAT, flipQuadEdges));
|
||||
}
|
||||
|
||||
btRayShape *ShapeBullet::create_shape_ray(real_t p_length) {
|
||||
return bulletnew(btRayShape(p_length));
|
||||
}
|
||||
|
||||
/* PLANE */
|
||||
|
||||
PlaneShapeBullet::PlaneShapeBullet()
|
||||
: ShapeBullet() {}
|
||||
|
||||
void PlaneShapeBullet::set_data(const Variant &p_data) {
|
||||
setup(p_data);
|
||||
}
|
||||
|
||||
Variant PlaneShapeBullet::get_data() const {
|
||||
return plane;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType PlaneShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_PLANE;
|
||||
}
|
||||
|
||||
void PlaneShapeBullet::setup(const Plane &p_plane) {
|
||||
plane = p_plane;
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *PlaneShapeBullet::create_bt_shape() {
|
||||
btVector3 btPlaneNormal;
|
||||
G_TO_B(plane.normal, btPlaneNormal);
|
||||
return prepare(PlaneShapeBullet::create_shape_plane(btPlaneNormal, plane.d));
|
||||
}
|
||||
|
||||
/* Sphere */
|
||||
|
||||
SphereShapeBullet::SphereShapeBullet()
|
||||
: ShapeBullet() {}
|
||||
|
||||
void SphereShapeBullet::set_data(const Variant &p_data) {
|
||||
setup(p_data);
|
||||
}
|
||||
|
||||
Variant SphereShapeBullet::get_data() const {
|
||||
return radius;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType SphereShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_SPHERE;
|
||||
}
|
||||
|
||||
void SphereShapeBullet::setup(real_t p_radius) {
|
||||
radius = p_radius;
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *SphereShapeBullet::create_bt_shape() {
|
||||
return prepare(ShapeBullet::create_shape_sphere(radius));
|
||||
}
|
||||
|
||||
/* Box */
|
||||
BoxShapeBullet::BoxShapeBullet()
|
||||
: ShapeBullet() {}
|
||||
|
||||
void BoxShapeBullet::set_data(const Variant &p_data) {
|
||||
setup(p_data);
|
||||
}
|
||||
|
||||
Variant BoxShapeBullet::get_data() const {
|
||||
Vector3 g_half_extents;
|
||||
B_TO_G(half_extents, g_half_extents);
|
||||
return g_half_extents;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType BoxShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_BOX;
|
||||
}
|
||||
|
||||
void BoxShapeBullet::setup(const Vector3 &p_half_extents) {
|
||||
G_TO_B(p_half_extents, half_extents);
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *BoxShapeBullet::create_bt_shape() {
|
||||
return prepare(ShapeBullet::create_shape_box(half_extents));
|
||||
}
|
||||
|
||||
/* Capsule */
|
||||
|
||||
CapsuleShapeBullet::CapsuleShapeBullet()
|
||||
: ShapeBullet() {}
|
||||
|
||||
void CapsuleShapeBullet::set_data(const Variant &p_data) {
|
||||
Dictionary d = p_data;
|
||||
ERR_FAIL_COND(!d.has("radius"));
|
||||
ERR_FAIL_COND(!d.has("height"));
|
||||
setup(d["height"], d["radius"]);
|
||||
}
|
||||
|
||||
Variant CapsuleShapeBullet::get_data() const {
|
||||
Dictionary d;
|
||||
d["radius"] = radius;
|
||||
d["height"] = height;
|
||||
return d;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType CapsuleShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_CAPSULE;
|
||||
}
|
||||
|
||||
void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) {
|
||||
radius = p_radius;
|
||||
height = p_height;
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *CapsuleShapeBullet::create_bt_shape() {
|
||||
return prepare(ShapeBullet::create_shape_capsule(radius, height));
|
||||
}
|
||||
|
||||
/* Convex polygon */
|
||||
|
||||
ConvexPolygonShapeBullet::ConvexPolygonShapeBullet()
|
||||
: ShapeBullet() {}
|
||||
|
||||
void ConvexPolygonShapeBullet::set_data(const Variant &p_data) {
|
||||
setup(p_data);
|
||||
}
|
||||
|
||||
void ConvexPolygonShapeBullet::get_vertices(Vector<Vector3> &out_vertices) {
|
||||
const int n_of_vertices = vertices.size();
|
||||
out_vertices.resize(n_of_vertices);
|
||||
for (int i = n_of_vertices - 1; 0 <= i; --i) {
|
||||
B_TO_G(vertices[i], out_vertices[i]);
|
||||
}
|
||||
}
|
||||
|
||||
Variant ConvexPolygonShapeBullet::get_data() const {
|
||||
ConvexPolygonShapeBullet *variable_self = const_cast<ConvexPolygonShapeBullet *>(this);
|
||||
Vector<Vector3> out_vertices;
|
||||
variable_self->get_vertices(out_vertices);
|
||||
return out_vertices;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType ConvexPolygonShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_CONVEX_POLYGON;
|
||||
}
|
||||
|
||||
void ConvexPolygonShapeBullet::setup(const Vector<Vector3> &p_vertices) {
|
||||
// Make a copy of verticies
|
||||
const int n_of_vertices = p_vertices.size();
|
||||
vertices.resize(n_of_vertices);
|
||||
for (int i = n_of_vertices - 1; 0 <= i; --i) {
|
||||
G_TO_B(p_vertices[i], vertices[i]);
|
||||
}
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape() {
|
||||
return prepare(ShapeBullet::create_shape_convex(vertices));
|
||||
}
|
||||
|
||||
/* Concave polygon */
|
||||
|
||||
ConcavePolygonShapeBullet::ConcavePolygonShapeBullet()
|
||||
: ShapeBullet(), meshShape(NULL) {}
|
||||
|
||||
ConcavePolygonShapeBullet::~ConcavePolygonShapeBullet() {
|
||||
if (meshShape) {
|
||||
delete meshShape->getMeshInterface();
|
||||
delete meshShape;
|
||||
}
|
||||
faces = PoolVector<Vector3>();
|
||||
}
|
||||
|
||||
void ConcavePolygonShapeBullet::set_data(const Variant &p_data) {
|
||||
setup(p_data);
|
||||
}
|
||||
|
||||
Variant ConcavePolygonShapeBullet::get_data() const {
|
||||
return faces;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType ConcavePolygonShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_CONCAVE_POLYGON;
|
||||
}
|
||||
|
||||
void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
|
||||
faces = p_faces;
|
||||
if (meshShape) {
|
||||
/// Clear previous created shape
|
||||
delete meshShape->getMeshInterface();
|
||||
bulletdelete(meshShape);
|
||||
}
|
||||
int src_face_count = faces.size();
|
||||
if (0 < src_face_count) {
|
||||
|
||||
btTriangleMesh *shapeInterface = bulletnew(btTriangleMesh);
|
||||
|
||||
// It counts the faces and assert the array contains the correct number of vertices.
|
||||
ERR_FAIL_COND(src_face_count % 3);
|
||||
src_face_count /= 3;
|
||||
PoolVector<Vector3>::Read r = p_faces.read();
|
||||
const Vector3 *facesr = r.ptr();
|
||||
|
||||
btVector3 supVec_0;
|
||||
btVector3 supVec_1;
|
||||
btVector3 supVec_2;
|
||||
for (int i = 0; i < src_face_count; ++i) {
|
||||
G_TO_B(facesr[i * 3], supVec_0);
|
||||
G_TO_B(facesr[i * 3 + 1], supVec_1);
|
||||
G_TO_B(facesr[i * 3 + 2], supVec_2);
|
||||
|
||||
shapeInterface->addTriangle(supVec_0, supVec_1, supVec_2);
|
||||
}
|
||||
|
||||
const bool useQuantizedAabbCompression = true;
|
||||
|
||||
meshShape = bulletnew(btBvhTriangleMeshShape(shapeInterface, useQuantizedAabbCompression));
|
||||
} else {
|
||||
meshShape = NULL;
|
||||
ERR_PRINT("The faces count are 0, the mesh shape cannot be created");
|
||||
}
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape() {
|
||||
btCollisionShape *cs = ShapeBullet::create_shape_concave(meshShape);
|
||||
if (!cs) {
|
||||
// This is necessary since if 0 faces the creation of concave return NULL
|
||||
cs = ShapeBullet::create_shape_empty();
|
||||
}
|
||||
return prepare(cs);
|
||||
}
|
||||
|
||||
/* Height map shape */
|
||||
|
||||
HeightMapShapeBullet::HeightMapShapeBullet()
|
||||
: ShapeBullet() {}
|
||||
|
||||
void HeightMapShapeBullet::set_data(const Variant &p_data) {
|
||||
ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
|
||||
Dictionary d = p_data;
|
||||
ERR_FAIL_COND(!d.has("width"));
|
||||
ERR_FAIL_COND(!d.has("depth"));
|
||||
ERR_FAIL_COND(!d.has("cell_size"));
|
||||
ERR_FAIL_COND(!d.has("heights"));
|
||||
|
||||
int l_width = d["width"];
|
||||
int l_depth = d["depth"];
|
||||
real_t l_cell_size = d["cell_size"];
|
||||
PoolVector<real_t> l_heights = d["heights"];
|
||||
|
||||
ERR_FAIL_COND(l_width <= 0);
|
||||
ERR_FAIL_COND(l_depth <= 0);
|
||||
ERR_FAIL_COND(l_cell_size <= CMP_EPSILON);
|
||||
ERR_FAIL_COND(l_heights.size() != (width * depth));
|
||||
setup(heights, width, depth, cell_size);
|
||||
}
|
||||
|
||||
Variant HeightMapShapeBullet::get_data() const {
|
||||
ERR_FAIL_V(Variant());
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType HeightMapShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_HEIGHTMAP;
|
||||
}
|
||||
|
||||
void HeightMapShapeBullet::setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size) {
|
||||
{ // Copy
|
||||
const int heights_size = p_heights.size();
|
||||
heights.resize(heights_size);
|
||||
PoolVector<real_t>::Read p_heights_r = p_heights.read();
|
||||
PoolVector<real_t>::Write heights_w = heights.write();
|
||||
for (int i = heights_size - 1; 0 <= i; --i) {
|
||||
heights_w[i] = p_heights_r[i];
|
||||
}
|
||||
}
|
||||
width = p_width;
|
||||
depth = p_depth;
|
||||
cell_size = p_cell_size;
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *HeightMapShapeBullet::create_bt_shape() {
|
||||
return prepare(ShapeBullet::create_shape_height_field(heights, width, depth, cell_size));
|
||||
}
|
||||
|
||||
/* Ray shape */
|
||||
RayShapeBullet::RayShapeBullet()
|
||||
: ShapeBullet(), length(1) {}
|
||||
|
||||
void RayShapeBullet::set_data(const Variant &p_data) {
|
||||
setup(p_data);
|
||||
}
|
||||
|
||||
Variant RayShapeBullet::get_data() const {
|
||||
return length;
|
||||
}
|
||||
|
||||
PhysicsServer::ShapeType RayShapeBullet::get_type() const {
|
||||
return PhysicsServer::SHAPE_RAY;
|
||||
}
|
||||
|
||||
void RayShapeBullet::setup(real_t p_length) {
|
||||
length = p_length;
|
||||
notifyShapeChanged();
|
||||
}
|
||||
|
||||
btCollisionShape *RayShapeBullet::create_bt_shape() {
|
||||
return prepare(ShapeBullet::create_shape_ray(length));
|
||||
}
|
225
modules/bullet/shape_bullet.h
Normal file
225
modules/bullet/shape_bullet.h
Normal file
|
@ -0,0 +1,225 @@
|
|||
/*************************************************************************/
|
||||
/* shape_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef SHAPE_BULLET_H
|
||||
#define SHAPE_BULLET_H
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "core/variant.h"
|
||||
#include "geometry.h"
|
||||
#include "rid_bullet.h"
|
||||
#include "servers/physics_server.h"
|
||||
|
||||
class ShapeBullet;
|
||||
class btCollisionShape;
|
||||
class ShapeOwnerBullet;
|
||||
class btBvhTriangleMeshShape;
|
||||
|
||||
class ShapeBullet : public RIDBullet {
|
||||
|
||||
Map<ShapeOwnerBullet *, int> owners;
|
||||
|
||||
protected:
|
||||
/// return self
|
||||
btCollisionShape *prepare(btCollisionShape *p_btShape) const;
|
||||
void notifyShapeChanged();
|
||||
|
||||
public:
|
||||
ShapeBullet();
|
||||
virtual ~ShapeBullet();
|
||||
|
||||
virtual btCollisionShape *create_bt_shape() = 0;
|
||||
|
||||
void add_owner(ShapeOwnerBullet *p_owner);
|
||||
void remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody = false);
|
||||
bool is_owner(ShapeOwnerBullet *p_owner) const;
|
||||
const Map<ShapeOwnerBullet *, int> &get_owners() const;
|
||||
|
||||
/// Setup the shape
|
||||
virtual void set_data(const Variant &p_data) = 0;
|
||||
virtual Variant get_data() const = 0;
|
||||
|
||||
virtual PhysicsServer::ShapeType get_type() const = 0;
|
||||
|
||||
public:
|
||||
static class btEmptyShape *create_shape_empty();
|
||||
static class btStaticPlaneShape *create_shape_plane(const btVector3 &planeNormal, btScalar planeConstant);
|
||||
static class btSphereShape *create_shape_sphere(btScalar radius);
|
||||
static class btBoxShape *create_shape_box(const btVector3 &boxHalfExtents);
|
||||
static class btCapsuleShapeZ *create_shape_capsule(btScalar radius, btScalar height);
|
||||
/// IMPORTANT: Remember to delete the shape interface by calling: delete my_shape->getMeshInterface();
|
||||
static class btConvexPointCloudShape *create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
|
||||
static class btScaledBvhTriangleMeshShape *create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
|
||||
static class btHeightfieldTerrainShape *create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
|
||||
static class btRayShape *create_shape_ray(real_t p_length);
|
||||
};
|
||||
|
||||
class PlaneShapeBullet : public ShapeBullet {
|
||||
|
||||
Plane plane;
|
||||
|
||||
public:
|
||||
PlaneShapeBullet();
|
||||
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(const Plane &p_plane);
|
||||
};
|
||||
|
||||
class SphereShapeBullet : public ShapeBullet {
|
||||
|
||||
real_t radius;
|
||||
|
||||
public:
|
||||
SphereShapeBullet();
|
||||
|
||||
_FORCE_INLINE_ real_t get_radius() { return radius; }
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(real_t p_radius);
|
||||
};
|
||||
|
||||
class BoxShapeBullet : public ShapeBullet {
|
||||
|
||||
btVector3 half_extents;
|
||||
|
||||
public:
|
||||
BoxShapeBullet();
|
||||
|
||||
_FORCE_INLINE_ const btVector3 &get_half_extents() { return half_extents; }
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(const Vector3 &p_half_extents);
|
||||
};
|
||||
|
||||
class CapsuleShapeBullet : public ShapeBullet {
|
||||
|
||||
real_t height;
|
||||
real_t radius;
|
||||
|
||||
public:
|
||||
CapsuleShapeBullet();
|
||||
|
||||
_FORCE_INLINE_ real_t get_height() { return height; }
|
||||
_FORCE_INLINE_ real_t get_radius() { return radius; }
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(real_t p_height, real_t p_radius);
|
||||
};
|
||||
|
||||
class ConvexPolygonShapeBullet : public ShapeBullet {
|
||||
|
||||
public:
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
|
||||
ConvexPolygonShapeBullet();
|
||||
|
||||
virtual void set_data(const Variant &p_data);
|
||||
void get_vertices(Vector<Vector3> &out_vertices);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(const Vector<Vector3> &p_vertices);
|
||||
};
|
||||
|
||||
class ConcavePolygonShapeBullet : public ShapeBullet {
|
||||
class btBvhTriangleMeshShape *meshShape;
|
||||
|
||||
public:
|
||||
PoolVector<Vector3> faces;
|
||||
|
||||
ConcavePolygonShapeBullet();
|
||||
virtual ~ConcavePolygonShapeBullet();
|
||||
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(PoolVector<Vector3> p_faces);
|
||||
};
|
||||
|
||||
class HeightMapShapeBullet : public ShapeBullet {
|
||||
|
||||
public:
|
||||
PoolVector<real_t> heights;
|
||||
int width;
|
||||
int depth;
|
||||
real_t cell_size;
|
||||
|
||||
HeightMapShapeBullet();
|
||||
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
|
||||
};
|
||||
|
||||
class RayShapeBullet : public ShapeBullet {
|
||||
|
||||
public:
|
||||
real_t length;
|
||||
|
||||
RayShapeBullet();
|
||||
|
||||
virtual void set_data(const Variant &p_data);
|
||||
virtual Variant get_data() const;
|
||||
virtual PhysicsServer::ShapeType get_type() const;
|
||||
virtual btCollisionShape *create_bt_shape();
|
||||
|
||||
private:
|
||||
void setup(real_t p_length);
|
||||
};
|
||||
#endif
|
32
modules/bullet/shape_owner_bullet.cpp
Normal file
32
modules/bullet/shape_owner_bullet.cpp
Normal file
|
@ -0,0 +1,32 @@
|
|||
/*************************************************************************/
|
||||
/* shape_owner_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "shape_owner_bullet.h"
|
52
modules/bullet/shape_owner_bullet.h
Normal file
52
modules/bullet/shape_owner_bullet.h
Normal file
|
@ -0,0 +1,52 @@
|
|||
/*************************************************************************/
|
||||
/* shape_owner_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef SHAPE_OWNER_BULLET_H
|
||||
#define SHAPE_OWNER_BULLET_H
|
||||
|
||||
#include "rid_bullet.h"
|
||||
|
||||
class ShapeBullet;
|
||||
class btCollisionShape;
|
||||
class CollisionObjectBullet;
|
||||
|
||||
/// Each clas that want to use Shapes must inherit this class
|
||||
/// E.G. BodyShape is a child of this
|
||||
class ShapeOwnerBullet {
|
||||
public:
|
||||
/// This is used to set new shape or replace existing
|
||||
//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
|
||||
virtual void on_shape_changed(const ShapeBullet *const p_shape) = 0;
|
||||
virtual void on_shapes_changed() = 0;
|
||||
virtual void remove_shape(class ShapeBullet *p_shape) = 0;
|
||||
virtual ~ShapeOwnerBullet() {}
|
||||
};
|
||||
#endif
|
385
modules/bullet/slider_joint_bullet.cpp
Normal file
385
modules/bullet/slider_joint_bullet.cpp
Normal file
|
@ -0,0 +1,385 @@
|
|||
/*************************************************************************/
|
||||
/* slider_joint_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "slider_joint_bullet.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "rigid_body_bullet.h"
|
||||
|
||||
SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB)
|
||||
: JointBullet() {
|
||||
btTransform btFrameA;
|
||||
G_TO_B(frameInA, btFrameA);
|
||||
if (rbB) {
|
||||
btTransform btFrameB;
|
||||
G_TO_B(frameInB, btFrameB);
|
||||
sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, true));
|
||||
|
||||
} else {
|
||||
sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), btFrameA, true));
|
||||
}
|
||||
setup(sliderConstraint);
|
||||
}
|
||||
|
||||
const RigidBodyBullet *SliderJointBullet::getRigidBodyA() const {
|
||||
return static_cast<RigidBodyBullet *>(sliderConstraint->getRigidBodyA().getUserPointer());
|
||||
}
|
||||
|
||||
const RigidBodyBullet *SliderJointBullet::getRigidBodyB() const {
|
||||
return static_cast<RigidBodyBullet *>(sliderConstraint->getRigidBodyB().getUserPointer());
|
||||
}
|
||||
|
||||
const Transform SliderJointBullet::getCalculatedTransformA() const {
|
||||
btTransform btTransform = sliderConstraint->getCalculatedTransformA();
|
||||
Transform gTrans;
|
||||
B_TO_G(btTransform, gTrans);
|
||||
return gTrans;
|
||||
}
|
||||
|
||||
const Transform SliderJointBullet::getCalculatedTransformB() const {
|
||||
btTransform btTransform = sliderConstraint->getCalculatedTransformB();
|
||||
Transform gTrans;
|
||||
B_TO_G(btTransform, gTrans);
|
||||
return gTrans;
|
||||
}
|
||||
|
||||
const Transform SliderJointBullet::getFrameOffsetA() const {
|
||||
btTransform btTransform = sliderConstraint->getFrameOffsetA();
|
||||
Transform gTrans;
|
||||
B_TO_G(btTransform, gTrans);
|
||||
return gTrans;
|
||||
}
|
||||
|
||||
const Transform SliderJointBullet::getFrameOffsetB() const {
|
||||
btTransform btTransform = sliderConstraint->getFrameOffsetB();
|
||||
Transform gTrans;
|
||||
B_TO_G(btTransform, gTrans);
|
||||
return gTrans;
|
||||
}
|
||||
|
||||
Transform SliderJointBullet::getFrameOffsetA() {
|
||||
btTransform btTransform = sliderConstraint->getFrameOffsetA();
|
||||
Transform gTrans;
|
||||
B_TO_G(btTransform, gTrans);
|
||||
return gTrans;
|
||||
}
|
||||
|
||||
Transform SliderJointBullet::getFrameOffsetB() {
|
||||
btTransform btTransform = sliderConstraint->getFrameOffsetB();
|
||||
Transform gTrans;
|
||||
B_TO_G(btTransform, gTrans);
|
||||
return gTrans;
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getLowerLinLimit() const {
|
||||
return sliderConstraint->getLowerLinLimit();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setLowerLinLimit(real_t lowerLimit) {
|
||||
sliderConstraint->setLowerLinLimit(lowerLimit);
|
||||
}
|
||||
real_t SliderJointBullet::getUpperLinLimit() const {
|
||||
return sliderConstraint->getUpperLinLimit();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setUpperLinLimit(real_t upperLimit) {
|
||||
sliderConstraint->setUpperLinLimit(upperLimit);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getLowerAngLimit() const {
|
||||
return sliderConstraint->getLowerAngLimit();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setLowerAngLimit(real_t lowerLimit) {
|
||||
sliderConstraint->setLowerAngLimit(lowerLimit);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getUpperAngLimit() const {
|
||||
return sliderConstraint->getUpperAngLimit();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setUpperAngLimit(real_t upperLimit) {
|
||||
sliderConstraint->setUpperAngLimit(upperLimit);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getSoftnessDirLin() const {
|
||||
return sliderConstraint->getSoftnessDirLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getRestitutionDirLin() const {
|
||||
return sliderConstraint->getRestitutionDirLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getDampingDirLin() const {
|
||||
return sliderConstraint->getDampingDirLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getSoftnessDirAng() const {
|
||||
return sliderConstraint->getSoftnessDirAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getRestitutionDirAng() const {
|
||||
return sliderConstraint->getRestitutionDirAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getDampingDirAng() const {
|
||||
return sliderConstraint->getDampingDirAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getSoftnessLimLin() const {
|
||||
return sliderConstraint->getSoftnessLimLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getRestitutionLimLin() const {
|
||||
return sliderConstraint->getRestitutionLimLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getDampingLimLin() const {
|
||||
return sliderConstraint->getDampingLimLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getSoftnessLimAng() const {
|
||||
return sliderConstraint->getSoftnessLimAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getRestitutionLimAng() const {
|
||||
return sliderConstraint->getRestitutionLimAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getDampingLimAng() const {
|
||||
return sliderConstraint->getDampingLimAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getSoftnessOrthoLin() const {
|
||||
return sliderConstraint->getSoftnessOrthoLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getRestitutionOrthoLin() const {
|
||||
return sliderConstraint->getRestitutionOrthoLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getDampingOrthoLin() const {
|
||||
return sliderConstraint->getDampingOrthoLin();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getSoftnessOrthoAng() const {
|
||||
return sliderConstraint->getSoftnessOrthoAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getRestitutionOrthoAng() const {
|
||||
return sliderConstraint->getRestitutionOrthoAng();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getDampingOrthoAng() const {
|
||||
return sliderConstraint->getDampingOrthoAng();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setSoftnessDirLin(real_t softnessDirLin) {
|
||||
sliderConstraint->setSoftnessDirLin(softnessDirLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setRestitutionDirLin(real_t restitutionDirLin) {
|
||||
sliderConstraint->setRestitutionDirLin(restitutionDirLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setDampingDirLin(real_t dampingDirLin) {
|
||||
sliderConstraint->setDampingDirLin(dampingDirLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setSoftnessDirAng(real_t softnessDirAng) {
|
||||
sliderConstraint->setSoftnessDirAng(softnessDirAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setRestitutionDirAng(real_t restitutionDirAng) {
|
||||
sliderConstraint->setRestitutionDirAng(restitutionDirAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setDampingDirAng(real_t dampingDirAng) {
|
||||
sliderConstraint->setDampingDirAng(dampingDirAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setSoftnessLimLin(real_t softnessLimLin) {
|
||||
sliderConstraint->setSoftnessLimLin(softnessLimLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setRestitutionLimLin(real_t restitutionLimLin) {
|
||||
sliderConstraint->setRestitutionLimLin(restitutionLimLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setDampingLimLin(real_t dampingLimLin) {
|
||||
sliderConstraint->setDampingLimLin(dampingLimLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setSoftnessLimAng(real_t softnessLimAng) {
|
||||
sliderConstraint->setSoftnessLimAng(softnessLimAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setRestitutionLimAng(real_t restitutionLimAng) {
|
||||
sliderConstraint->setRestitutionLimAng(restitutionLimAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setDampingLimAng(real_t dampingLimAng) {
|
||||
sliderConstraint->setDampingLimAng(dampingLimAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setSoftnessOrthoLin(real_t softnessOrthoLin) {
|
||||
sliderConstraint->setSoftnessOrthoLin(softnessOrthoLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setRestitutionOrthoLin(real_t restitutionOrthoLin) {
|
||||
sliderConstraint->setRestitutionOrthoLin(restitutionOrthoLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setDampingOrthoLin(real_t dampingOrthoLin) {
|
||||
sliderConstraint->setDampingOrthoLin(dampingOrthoLin);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setSoftnessOrthoAng(real_t softnessOrthoAng) {
|
||||
sliderConstraint->setSoftnessOrthoAng(softnessOrthoAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setRestitutionOrthoAng(real_t restitutionOrthoAng) {
|
||||
sliderConstraint->setRestitutionOrthoAng(restitutionOrthoAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setDampingOrthoAng(real_t dampingOrthoAng) {
|
||||
sliderConstraint->setDampingOrthoAng(dampingOrthoAng);
|
||||
}
|
||||
|
||||
void SliderJointBullet::setPoweredLinMotor(bool onOff) {
|
||||
sliderConstraint->setPoweredLinMotor(onOff);
|
||||
}
|
||||
|
||||
bool SliderJointBullet::getPoweredLinMotor() {
|
||||
return sliderConstraint->getPoweredLinMotor();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setTargetLinMotorVelocity(real_t targetLinMotorVelocity) {
|
||||
sliderConstraint->setTargetLinMotorVelocity(targetLinMotorVelocity);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getTargetLinMotorVelocity() {
|
||||
return sliderConstraint->getTargetLinMotorVelocity();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setMaxLinMotorForce(real_t maxLinMotorForce) {
|
||||
sliderConstraint->setMaxLinMotorForce(maxLinMotorForce);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getMaxLinMotorForce() {
|
||||
return sliderConstraint->getMaxLinMotorForce();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setPoweredAngMotor(bool onOff) {
|
||||
sliderConstraint->setPoweredAngMotor(onOff);
|
||||
}
|
||||
|
||||
bool SliderJointBullet::getPoweredAngMotor() {
|
||||
return sliderConstraint->getPoweredAngMotor();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setTargetAngMotorVelocity(real_t targetAngMotorVelocity) {
|
||||
sliderConstraint->setTargetAngMotorVelocity(targetAngMotorVelocity);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getTargetAngMotorVelocity() {
|
||||
return sliderConstraint->getTargetAngMotorVelocity();
|
||||
}
|
||||
|
||||
void SliderJointBullet::setMaxAngMotorForce(real_t maxAngMotorForce) {
|
||||
sliderConstraint->setMaxAngMotorForce(maxAngMotorForce);
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getMaxAngMotorForce() {
|
||||
return sliderConstraint->getMaxAngMotorForce();
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::getLinearPos() {
|
||||
return sliderConstraint->getLinearPos();
|
||||
;
|
||||
}
|
||||
|
||||
void SliderJointBullet::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: setUpperLinLimit(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: setLowerLinLimit(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: setSoftnessLimLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: setRestitutionLimLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: setDampingLimLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: setSoftnessDirLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: setRestitutionDirLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: setDampingDirLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: setSoftnessOrthoLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: setRestitutionOrthoLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: setDampingOrthoLin(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: setUpperAngLimit(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: setLowerAngLimit(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: setSoftnessLimAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: setRestitutionLimAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: setDampingLimAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: setSoftnessDirAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: setRestitutionDirAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: setDampingDirAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: setSoftnessOrthoAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: setRestitutionOrthoAng(p_value); break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: setDampingOrthoAng(p_value); break;
|
||||
}
|
||||
}
|
||||
|
||||
real_t SliderJointBullet::get_param(PhysicsServer::SliderJointParam p_param) const {
|
||||
switch (p_param) {
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return getUpperLinLimit();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return getLowerLinLimit();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return getSoftnessLimLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return getRestitutionLimLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return getDampingLimLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return getSoftnessDirLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return getRestitutionDirLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return getDampingDirLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return getSoftnessOrthoLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return getRestitutionOrthoLin();
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return getDampingOrthoLin();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return getUpperAngLimit();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return getLowerAngLimit();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return getSoftnessLimAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return getRestitutionLimAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return getDampingLimAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return getSoftnessDirAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return getRestitutionDirAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return getDampingDirAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return getSoftnessOrthoAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return getRestitutionOrthoAng();
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return getDampingOrthoAng();
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
118
modules/bullet/slider_joint_bullet.h
Normal file
118
modules/bullet/slider_joint_bullet.h
Normal file
|
@ -0,0 +1,118 @@
|
|||
/*************************************************************************/
|
||||
/* slider_joint_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef SLIDER_JOINT_BULLET_H
|
||||
#define SLIDER_JOINT_BULLET_H
|
||||
|
||||
#include "joint_bullet.h"
|
||||
|
||||
class RigidBodyBullet;
|
||||
|
||||
class SliderJointBullet : public JointBullet {
|
||||
class btSliderConstraint *sliderConstraint;
|
||||
|
||||
public:
|
||||
/// Reference frame is A
|
||||
SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB);
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
|
||||
|
||||
const RigidBodyBullet *getRigidBodyA() const;
|
||||
const RigidBodyBullet *getRigidBodyB() const;
|
||||
const Transform getCalculatedTransformA() const;
|
||||
const Transform getCalculatedTransformB() const;
|
||||
const Transform getFrameOffsetA() const;
|
||||
const Transform getFrameOffsetB() const;
|
||||
Transform getFrameOffsetA();
|
||||
Transform getFrameOffsetB();
|
||||
real_t getLowerLinLimit() const;
|
||||
void setLowerLinLimit(real_t lowerLimit);
|
||||
real_t getUpperLinLimit() const;
|
||||
void setUpperLinLimit(real_t upperLimit);
|
||||
real_t getLowerAngLimit() const;
|
||||
void setLowerAngLimit(real_t lowerLimit);
|
||||
real_t getUpperAngLimit() const;
|
||||
void setUpperAngLimit(real_t upperLimit);
|
||||
|
||||
real_t getSoftnessDirLin() const;
|
||||
real_t getRestitutionDirLin() const;
|
||||
real_t getDampingDirLin() const;
|
||||
real_t getSoftnessDirAng() const;
|
||||
real_t getRestitutionDirAng() const;
|
||||
real_t getDampingDirAng() const;
|
||||
real_t getSoftnessLimLin() const;
|
||||
real_t getRestitutionLimLin() const;
|
||||
real_t getDampingLimLin() const;
|
||||
real_t getSoftnessLimAng() const;
|
||||
real_t getRestitutionLimAng() const;
|
||||
real_t getDampingLimAng() const;
|
||||
real_t getSoftnessOrthoLin() const;
|
||||
real_t getRestitutionOrthoLin() const;
|
||||
real_t getDampingOrthoLin() const;
|
||||
real_t getSoftnessOrthoAng() const;
|
||||
real_t getRestitutionOrthoAng() const;
|
||||
real_t getDampingOrthoAng() const;
|
||||
void setSoftnessDirLin(real_t softnessDirLin);
|
||||
void setRestitutionDirLin(real_t restitutionDirLin);
|
||||
void setDampingDirLin(real_t dampingDirLin);
|
||||
void setSoftnessDirAng(real_t softnessDirAng);
|
||||
void setRestitutionDirAng(real_t restitutionDirAng);
|
||||
void setDampingDirAng(real_t dampingDirAng);
|
||||
void setSoftnessLimLin(real_t softnessLimLin);
|
||||
void setRestitutionLimLin(real_t restitutionLimLin);
|
||||
void setDampingLimLin(real_t dampingLimLin);
|
||||
void setSoftnessLimAng(real_t softnessLimAng);
|
||||
void setRestitutionLimAng(real_t restitutionLimAng);
|
||||
void setDampingLimAng(real_t dampingLimAng);
|
||||
void setSoftnessOrthoLin(real_t softnessOrthoLin);
|
||||
void setRestitutionOrthoLin(real_t restitutionOrthoLin);
|
||||
void setDampingOrthoLin(real_t dampingOrthoLin);
|
||||
void setSoftnessOrthoAng(real_t softnessOrthoAng);
|
||||
void setRestitutionOrthoAng(real_t restitutionOrthoAng);
|
||||
void setDampingOrthoAng(real_t dampingOrthoAng);
|
||||
void setPoweredLinMotor(bool onOff);
|
||||
bool getPoweredLinMotor();
|
||||
void setTargetLinMotorVelocity(real_t targetLinMotorVelocity);
|
||||
real_t getTargetLinMotorVelocity();
|
||||
void setMaxLinMotorForce(real_t maxLinMotorForce);
|
||||
real_t getMaxLinMotorForce();
|
||||
void setPoweredAngMotor(bool onOff);
|
||||
bool getPoweredAngMotor();
|
||||
void setTargetAngMotorVelocity(real_t targetAngMotorVelocity);
|
||||
real_t getTargetAngMotorVelocity();
|
||||
void setMaxAngMotorForce(real_t maxAngMotorForce);
|
||||
real_t getMaxAngMotorForce();
|
||||
real_t getLinearPos();
|
||||
|
||||
void set_param(PhysicsServer::SliderJointParam p_param, real_t p_value);
|
||||
real_t get_param(PhysicsServer::SliderJointParam p_param) const;
|
||||
};
|
||||
#endif
|
303
modules/bullet/soft_body_bullet.cpp
Normal file
303
modules/bullet/soft_body_bullet.cpp
Normal file
|
@ -0,0 +1,303 @@
|
|||
/*************************************************************************/
|
||||
/* soft_body_bullet.cpp */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "soft_body_bullet.h"
|
||||
#include "bullet_types_converter.h"
|
||||
#include "bullet_utilities.h"
|
||||
#include "space_bullet.h"
|
||||
|
||||
#include "scene/3d/immediate_geometry.h"
|
||||
|
||||
SoftBodyBullet::SoftBodyBullet()
|
||||
: CollisionObjectBullet(CollisionObjectBullet::TYPE_SOFT_BODY), mass(1), simulation_precision(5), stiffness(0.5f), pressure_coefficient(50), damping_coefficient(0.005), drag_coefficient(0.005), bt_soft_body(NULL), soft_shape_type(SOFT_SHAPETYPE_NONE), isScratched(false), soft_body_shape_data(NULL) {
|
||||
|
||||
test_geometry = memnew(ImmediateGeometry);
|
||||
|
||||
red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
|
||||
red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
|
||||
red_mat->set_line_width(20.0);
|
||||
red_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
|
||||
red_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
|
||||
red_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
|
||||
red_mat->set_albedo(Color(1, 0, 0, 1));
|
||||
test_geometry->set_material_override(red_mat);
|
||||
|
||||
test_is_in_scene = false;
|
||||
}
|
||||
|
||||
SoftBodyBullet::~SoftBodyBullet() {
|
||||
bulletdelete(soft_body_shape_data);
|
||||
}
|
||||
|
||||
void SoftBodyBullet::reload_body() {
|
||||
if (space) {
|
||||
space->remove_soft_body(this);
|
||||
space->add_soft_body(this);
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_space(SpaceBullet *p_space) {
|
||||
if (space) {
|
||||
isScratched = false;
|
||||
|
||||
// Remove this object from the physics world
|
||||
space->remove_soft_body(this);
|
||||
}
|
||||
|
||||
space = p_space;
|
||||
|
||||
if (space) {
|
||||
space->add_soft_body(this);
|
||||
}
|
||||
|
||||
reload_soft_body();
|
||||
}
|
||||
|
||||
void SoftBodyBullet::dispatch_callbacks() {
|
||||
if (!bt_soft_body) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!test_is_in_scene) {
|
||||
test_is_in_scene = true;
|
||||
SceneTree::get_singleton()->get_current_scene()->add_child(test_geometry);
|
||||
}
|
||||
|
||||
test_geometry->clear();
|
||||
test_geometry->begin(Mesh::PRIMITIVE_LINES, NULL);
|
||||
bool first = true;
|
||||
Vector3 pos;
|
||||
for (int i = 0; i < bt_soft_body->m_nodes.size(); ++i) {
|
||||
const btSoftBody::Node &n = bt_soft_body->m_nodes[i];
|
||||
B_TO_G(n.m_x, pos);
|
||||
test_geometry->add_vertex(pos);
|
||||
if (!first) {
|
||||
test_geometry->add_vertex(pos);
|
||||
} else {
|
||||
first = false;
|
||||
}
|
||||
}
|
||||
test_geometry->end();
|
||||
}
|
||||
|
||||
void SoftBodyBullet::on_collision_filters_change() {
|
||||
}
|
||||
|
||||
void SoftBodyBullet::on_collision_checker_start() {
|
||||
}
|
||||
|
||||
void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {
|
||||
}
|
||||
|
||||
void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num) {
|
||||
|
||||
TrimeshSoftShapeData *shape_data = bulletnew(TrimeshSoftShapeData);
|
||||
shape_data->m_triangles_indices = p_indices;
|
||||
shape_data->m_vertices = p_vertices;
|
||||
shape_data->m_triangles_num = p_triangles_num;
|
||||
|
||||
set_body_shape_data(shape_data, SOFT_SHAPE_TYPE_TRIMESH);
|
||||
reload_soft_body();
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_body_shape_data(SoftShapeData *p_soft_shape_data, SoftShapeType p_type) {
|
||||
bulletdelete(soft_body_shape_data);
|
||||
soft_body_shape_data = p_soft_shape_data;
|
||||
soft_shape_type = p_type;
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_transform(const Transform &p_transform) {
|
||||
transform = p_transform;
|
||||
if (bt_soft_body) {
|
||||
// TODO the softbody set new transform considering the current transform as center of world
|
||||
// like if it's local transform, so I must fix this by setting nwe transform considering the old
|
||||
btTransform bt_trans;
|
||||
G_TO_B(transform, bt_trans);
|
||||
//bt_soft_body->transform(bt_trans);
|
||||
}
|
||||
}
|
||||
|
||||
const Transform &SoftBodyBullet::get_transform() const {
|
||||
return transform;
|
||||
}
|
||||
|
||||
void SoftBodyBullet::get_first_node_origin(btVector3 &p_out_origin) const {
|
||||
if (bt_soft_body && bt_soft_body->m_nodes.size()) {
|
||||
p_out_origin = bt_soft_body->m_nodes[0].m_x;
|
||||
} else {
|
||||
p_out_origin.setZero();
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_activation_state(bool p_active) {
|
||||
if (p_active) {
|
||||
bt_soft_body->setActivationState(ACTIVE_TAG);
|
||||
} else {
|
||||
bt_soft_body->setActivationState(WANTS_DEACTIVATION);
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_mass(real_t p_val) {
|
||||
if (0 >= p_val) {
|
||||
p_val = 1;
|
||||
}
|
||||
mass = p_val;
|
||||
if (bt_soft_body) {
|
||||
bt_soft_body->setTotalMass(mass);
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_stiffness(real_t p_val) {
|
||||
stiffness = p_val;
|
||||
if (bt_soft_body) {
|
||||
mat0->m_kAST = stiffness;
|
||||
mat0->m_kLST = stiffness;
|
||||
mat0->m_kVST = stiffness;
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_simulation_precision(int p_val) {
|
||||
simulation_precision = p_val;
|
||||
if (bt_soft_body) {
|
||||
bt_soft_body->m_cfg.piterations = simulation_precision;
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_pressure_coefficient(real_t p_val) {
|
||||
pressure_coefficient = p_val;
|
||||
if (bt_soft_body) {
|
||||
bt_soft_body->m_cfg.kPR = pressure_coefficient;
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_damping_coefficient(real_t p_val) {
|
||||
damping_coefficient = p_val;
|
||||
if (bt_soft_body) {
|
||||
bt_soft_body->m_cfg.kDP = damping_coefficient;
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::set_drag_coefficient(real_t p_val) {
|
||||
drag_coefficient = p_val;
|
||||
if (bt_soft_body) {
|
||||
bt_soft_body->m_cfg.kDG = drag_coefficient;
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::reload_soft_body() {
|
||||
|
||||
destroy_soft_body();
|
||||
create_soft_body();
|
||||
|
||||
if (bt_soft_body) {
|
||||
|
||||
// TODO the softbody set new transform considering the current transform as center of world
|
||||
// like if it's local transform, so I must fix this by setting nwe transform considering the old
|
||||
btTransform bt_trans;
|
||||
G_TO_B(transform, bt_trans);
|
||||
bt_soft_body->transform(bt_trans);
|
||||
|
||||
bt_soft_body->generateBendingConstraints(2, mat0);
|
||||
mat0->m_kAST = stiffness;
|
||||
mat0->m_kLST = stiffness;
|
||||
mat0->m_kVST = stiffness;
|
||||
|
||||
bt_soft_body->m_cfg.piterations = simulation_precision;
|
||||
bt_soft_body->m_cfg.kDP = damping_coefficient;
|
||||
bt_soft_body->m_cfg.kDG = drag_coefficient;
|
||||
bt_soft_body->m_cfg.kPR = pressure_coefficient;
|
||||
bt_soft_body->setTotalMass(mass);
|
||||
}
|
||||
if (space) {
|
||||
// TODO remove this please
|
||||
space->add_soft_body(this);
|
||||
}
|
||||
}
|
||||
|
||||
void SoftBodyBullet::create_soft_body() {
|
||||
if (!space || !soft_body_shape_data) {
|
||||
return;
|
||||
}
|
||||
ERR_FAIL_COND(!space->is_using_soft_world());
|
||||
switch (soft_shape_type) {
|
||||
case SOFT_SHAPE_TYPE_TRIMESH: {
|
||||
TrimeshSoftShapeData *trimesh_data = static_cast<TrimeshSoftShapeData *>(soft_body_shape_data);
|
||||
|
||||
Vector<int> indices;
|
||||
Vector<btScalar> vertices;
|
||||
|
||||
int i;
|
||||
const int indices_size = trimesh_data->m_triangles_indices.size();
|
||||
const int vertices_size = trimesh_data->m_vertices.size();
|
||||
indices.resize(indices_size);
|
||||
vertices.resize(vertices_size * 3);
|
||||
|
||||
PoolVector<int>::Read i_r = trimesh_data->m_triangles_indices.read();
|
||||
for (i = 0; i < indices_size; ++i) {
|
||||
indices[i] = i_r[i];
|
||||
}
|
||||
i_r = PoolVector<int>::Read();
|
||||
|
||||
PoolVector<Vector3>::Read f_r = trimesh_data->m_vertices.read();
|
||||
for (int j = i = 0; i < vertices_size; ++i, j += 3) {
|
||||
vertices[j + 0] = f_r[i][0];
|
||||
vertices[j + 1] = f_r[i][1];
|
||||
vertices[j + 2] = f_r[i][2];
|
||||
}
|
||||
f_r = PoolVector<Vector3>::Read();
|
||||
|
||||
bt_soft_body = btSoftBodyHelpers::CreateFromTriMesh(*space->get_soft_body_world_info(), vertices.ptr(), indices.ptr(), trimesh_data->m_triangles_num);
|
||||
} break;
|
||||
default:
|
||||
ERR_PRINT("Shape type not supported");
|
||||
return;
|
||||
}
|
||||
|
||||
setupBulletCollisionObject(bt_soft_body);
|
||||
bt_soft_body->getCollisionShape()->setMargin(0.001f);
|
||||
bt_soft_body->setCollisionFlags(bt_soft_body->getCollisionFlags() & (~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)));
|
||||
mat0 = bt_soft_body->appendMaterial();
|
||||
}
|
||||
|
||||
void SoftBodyBullet::destroy_soft_body() {
|
||||
if (space) {
|
||||
/// This step is required to assert that the body is not into the world during deletion
|
||||
/// This step is required since to change the body shape the body must be re-created.
|
||||
/// Here is handled the case when the body is assigned into a world and the body
|
||||
/// shape is changed.
|
||||
space->remove_soft_body(this);
|
||||
}
|
||||
destroyBulletCollisionObject();
|
||||
bt_soft_body = NULL;
|
||||
}
|
136
modules/bullet/soft_body_bullet.h
Normal file
136
modules/bullet/soft_body_bullet.h
Normal file
|
@ -0,0 +1,136 @@
|
|||
/*************************************************************************/
|
||||
/* soft_body_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef SOFT_BODY_BULLET_H
|
||||
#define SOFT_BODY_BULLET_H
|
||||
|
||||
#ifdef None
|
||||
/// This is required to remove the macro None defined by x11 compiler because this word "None" is used internally by Bullet
|
||||
#undef None
|
||||
#define x11_None 0L
|
||||
#endif
|
||||
|
||||
#include "BulletSoftBody/btSoftBodyHelpers.h"
|
||||
#include "collision_object_bullet.h"
|
||||
|
||||
#ifdef x11_None
|
||||
/// This is required to re add the macro None defined by x11 compiler
|
||||
#undef x11_None
|
||||
#define None 0L
|
||||
#endif
|
||||
|
||||
#include "scene/resources/material.h" // TODO remove thsi please
|
||||
|
||||
struct SoftShapeData {};
|
||||
struct TrimeshSoftShapeData : public SoftShapeData {
|
||||
PoolVector<int> m_triangles_indices;
|
||||
PoolVector<Vector3> m_vertices;
|
||||
int m_triangles_num;
|
||||
};
|
||||
|
||||
class SoftBodyBullet : public CollisionObjectBullet {
|
||||
public:
|
||||
enum SoftShapeType {
|
||||
SOFT_SHAPETYPE_NONE = 0,
|
||||
SOFT_SHAPE_TYPE_TRIMESH
|
||||
};
|
||||
|
||||
private:
|
||||
btSoftBody *bt_soft_body;
|
||||
btSoftBody::Material *mat0; // This is just a copy of pointer managed by btSoftBody
|
||||
SoftShapeType soft_shape_type;
|
||||
bool isScratched;
|
||||
|
||||
SoftShapeData *soft_body_shape_data;
|
||||
|
||||
Transform transform;
|
||||
int simulation_precision;
|
||||
real_t mass;
|
||||
real_t stiffness; // [0,1]
|
||||
real_t pressure_coefficient; // [-inf,+inf]
|
||||
real_t damping_coefficient; // [0,1]
|
||||
real_t drag_coefficient; // [0,1]
|
||||
|
||||
class ImmediateGeometry *test_geometry; // TODO remove this please
|
||||
Ref<SpatialMaterial> red_mat; // TODO remove this please
|
||||
bool test_is_in_scene; // TODO remove this please
|
||||
|
||||
public:
|
||||
SoftBodyBullet();
|
||||
~SoftBodyBullet();
|
||||
|
||||
virtual void reload_body();
|
||||
virtual void set_space(SpaceBullet *p_space);
|
||||
|
||||
virtual void dispatch_callbacks();
|
||||
virtual void on_collision_filters_change();
|
||||
virtual void on_collision_checker_start();
|
||||
virtual void on_enter_area(AreaBullet *p_area);
|
||||
virtual void on_exit_area(AreaBullet *p_area);
|
||||
|
||||
_FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; }
|
||||
|
||||
void set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num);
|
||||
void set_body_shape_data(SoftShapeData *p_soft_shape_data, SoftShapeType p_type);
|
||||
|
||||
void set_transform(const Transform &p_transform);
|
||||
/// This function doesn't return the exact COM transform.
|
||||
/// It returns the origin only of first node (vertice) of current soft body
|
||||
/// ---
|
||||
/// The soft body doesn't have a fixed center of mass, but is a group of nodes (vertices)
|
||||
/// that each has its own position in the world.
|
||||
/// For this reason return the correct COM is not so simple and must be calculate
|
||||
/// Check this to improve this function http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=8803
|
||||
const Transform &get_transform() const;
|
||||
void get_first_node_origin(btVector3 &p_out_origin) const;
|
||||
|
||||
void set_activation_state(bool p_active);
|
||||
|
||||
void set_mass(real_t p_val);
|
||||
_FORCE_INLINE_ real_t get_mass() const { return mass; }
|
||||
void set_stiffness(real_t p_val);
|
||||
_FORCE_INLINE_ real_t get_stiffness() const { return stiffness; }
|
||||
void set_simulation_precision(int p_val);
|
||||
_FORCE_INLINE_ int get_simulation_precision() const { return simulation_precision; }
|
||||
void set_pressure_coefficient(real_t p_val);
|
||||
_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
|
||||
void set_damping_coefficient(real_t p_val);
|
||||
_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
|
||||
void set_drag_coefficient(real_t p_val);
|
||||
_FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
|
||||
|
||||
private:
|
||||
void reload_soft_body();
|
||||
void create_soft_body();
|
||||
void destroy_soft_body();
|
||||
};
|
||||
|
||||
#endif // SOFT_BODY_BULLET_H
|
1163
modules/bullet/space_bullet.cpp
Normal file
1163
modules/bullet/space_bullet.cpp
Normal file
File diff suppressed because it is too large
Load diff
176
modules/bullet/space_bullet.h
Normal file
176
modules/bullet/space_bullet.h
Normal file
|
@ -0,0 +1,176 @@
|
|||
/*************************************************************************/
|
||||
/* space_bullet.h */
|
||||
/* Author: AndreaCatania */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* http://www.godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
|
||||
/* */
|
||||
/* Permission is hereby granted, free of charge, to any person obtaining */
|
||||
/* a copy of this software and associated documentation files (the */
|
||||
/* "Software"), to deal in the Software without restriction, including */
|
||||
/* without limitation the rights to use, copy, modify, merge, publish, */
|
||||
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
||||
/* permit persons to whom the Software is furnished to do so, subject to */
|
||||
/* the following conditions: */
|
||||
/* */
|
||||
/* The above copyright notice and this permission notice shall be */
|
||||
/* included in all copies or substantial portions of the Software. */
|
||||
/* */
|
||||
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
||||
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
||||
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
||||
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
||||
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
||||
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
||||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef SPACE_BULLET_H
|
||||
#define SPACE_BULLET_H
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "core/variant.h"
|
||||
#include "core/vector.h"
|
||||
#include "godot_result_callbacks.h"
|
||||
#include "rid_bullet.h"
|
||||
#include "servers/physics_server.h"
|
||||
|
||||
class AreaBullet;
|
||||
class btBroadphaseInterface;
|
||||
class btCollisionDispatcher;
|
||||
class btConstraintSolver;
|
||||
class btDefaultCollisionConfiguration;
|
||||
class btDynamicsWorld;
|
||||
class btDiscreteDynamicsWorld;
|
||||
class btEmptyShape;
|
||||
class btGhostPairCallback;
|
||||
class btSoftRigidDynamicsWorld;
|
||||
class btSoftBodyWorldInfo;
|
||||
class ConstraintBullet;
|
||||
class CollisionObjectBullet;
|
||||
class RigidBodyBullet;
|
||||
class SpaceBullet;
|
||||
class SoftBodyBullet;
|
||||
|
||||
class BulletPhysicsDirectSpaceState : public PhysicsDirectSpaceState {
|
||||
GDCLASS(BulletPhysicsDirectSpaceState, PhysicsDirectSpaceState)
|
||||
private:
|
||||
SpaceBullet *space;
|
||||
|
||||
public:
|
||||
BulletPhysicsDirectSpaceState(SpaceBullet *p_space);
|
||||
|
||||
virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
|
||||
virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false);
|
||||
virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
|
||||
virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL);
|
||||
/// Returns the list of contacts pairs in this order: Local contact, other body contact
|
||||
virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
|
||||
virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
|
||||
virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const;
|
||||
};
|
||||
|
||||
class SpaceBullet : public RIDBullet {
|
||||
private:
|
||||
friend class AreaBullet;
|
||||
friend void onBulletTickCallback(btDynamicsWorld *world, btScalar timeStep);
|
||||
friend class BulletPhysicsDirectSpaceState;
|
||||
|
||||
btBroadphaseInterface *broadphase;
|
||||
btDefaultCollisionConfiguration *collisionConfiguration;
|
||||
btCollisionDispatcher *dispatcher;
|
||||
btConstraintSolver *solver;
|
||||
btDiscreteDynamicsWorld *dynamicsWorld;
|
||||
btGhostPairCallback *ghostPairCallback;
|
||||
GodotFilterCallback *godotFilterCallback;
|
||||
btSoftBodyWorldInfo *soft_body_world_info;
|
||||
|
||||
BulletPhysicsDirectSpaceState *direct_access;
|
||||
Vector3 gravityDirection;
|
||||
real_t gravityMagnitude;
|
||||
|
||||
Vector<AreaBullet *> areas;
|
||||
|
||||
Vector<Vector3> contactDebug;
|
||||
int contactDebugCount;
|
||||
|
||||
public:
|
||||
SpaceBullet(bool p_create_soft_world);
|
||||
virtual ~SpaceBullet();
|
||||
|
||||
void flush_queries();
|
||||
void step(real_t p_delta_time);
|
||||
|
||||
_FORCE_INLINE_ btCollisionDispatcher *get_dispatcher() { return dispatcher; }
|
||||
_FORCE_INLINE_ btSoftBodyWorldInfo *get_soft_body_world_info() { return soft_body_world_info; }
|
||||
_FORCE_INLINE_ bool is_using_soft_world() { return soft_body_world_info; }
|
||||
|
||||
/// Used to set some parameters to Bullet world
|
||||
/// @param p_param:
|
||||
/// AREA_PARAM_GRAVITY to set the gravity magnitude of entire world
|
||||
/// AREA_PARAM_GRAVITY_VECTOR to set the gravity direction of entire world
|
||||
void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value);
|
||||
/// Used to get some parameters to Bullet world
|
||||
/// @param p_param:
|
||||
/// AREA_PARAM_GRAVITY to get the gravity magnitude of entire world
|
||||
/// AREA_PARAM_GRAVITY_VECTOR to get the gravity direction of entire world
|
||||
Variant get_param(PhysicsServer::AreaParameter p_param);
|
||||
|
||||
void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
|
||||
real_t get_param(PhysicsServer::SpaceParameter p_param);
|
||||
|
||||
void add_area(AreaBullet *p_area);
|
||||
void remove_area(AreaBullet *p_area);
|
||||
void reload_collision_filters(AreaBullet *p_area);
|
||||
|
||||
void add_rigid_body(RigidBodyBullet *p_body);
|
||||
void remove_rigid_body(RigidBodyBullet *p_body);
|
||||
void reload_collision_filters(RigidBodyBullet *p_body);
|
||||
|
||||
void add_soft_body(SoftBodyBullet *p_body);
|
||||
void remove_soft_body(SoftBodyBullet *p_body);
|
||||
void reload_collision_filters(SoftBodyBullet *p_body);
|
||||
|
||||
void add_constraint(ConstraintBullet *p_constraint, bool disableCollisionsBetweenLinkedBodies = false);
|
||||
void remove_constraint(ConstraintBullet *p_constraint);
|
||||
|
||||
int get_num_collision_objects() const;
|
||||
void remove_all_collision_objects();
|
||||
|
||||
BulletPhysicsDirectSpaceState *get_direct_state();
|
||||
|
||||
void set_debug_contacts(int p_amount) { contactDebug.resize(p_amount); }
|
||||
_FORCE_INLINE_ bool is_debugging_contacts() const { return !contactDebug.empty(); }
|
||||
_FORCE_INLINE_ void reset_debug_contact_count() {
|
||||
contactDebugCount = 0;
|
||||
}
|
||||
_FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) {
|
||||
if (contactDebugCount < contactDebug.size()) contactDebug[contactDebugCount++] = p_contact;
|
||||
}
|
||||
_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contactDebug; }
|
||||
_FORCE_INLINE_ int get_debug_contact_count() { return contactDebugCount; }
|
||||
|
||||
const Vector3 &get_gravity_direction() const { return gravityDirection; }
|
||||
real_t get_gravity_magnitude() const { return gravityMagnitude; }
|
||||
|
||||
void update_gravity();
|
||||
|
||||
bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result);
|
||||
|
||||
private:
|
||||
void create_empty_world(bool p_create_soft_world);
|
||||
void destroy_world();
|
||||
void check_ghost_overlaps();
|
||||
void check_body_collision();
|
||||
|
||||
bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_maxPenetrationDepth, btScalar p_depenetration_speed, btVector3 &out_recover_position);
|
||||
};
|
||||
#endif
|
|
@ -357,7 +357,7 @@ public:
|
|||
BODY_MODE_STATIC,
|
||||
BODY_MODE_KINEMATIC,
|
||||
BODY_MODE_RIGID,
|
||||
//BODY_MODE_SOFT
|
||||
BODY_MODE_SOFT,
|
||||
BODY_MODE_CHARACTER
|
||||
};
|
||||
|
||||
|
|
40
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h
vendored
Normal file
40
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h
vendored
Normal file
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_BROADPHASE_CALLBACK_H
|
||||
#define B3_BROADPHASE_CALLBACK_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
struct b3BroadphaseProxy;
|
||||
|
||||
|
||||
struct b3BroadphaseAabbCallback
|
||||
{
|
||||
virtual ~b3BroadphaseAabbCallback() {}
|
||||
virtual bool process(const b3BroadphaseProxy* proxy) = 0;
|
||||
};
|
||||
|
||||
|
||||
struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
|
||||
{
|
||||
///added some cached data to accelerate ray-AABB tests
|
||||
b3Vector3 m_rayDirectionInverse;
|
||||
unsigned int m_signs[3];
|
||||
b3Scalar m_lambda_max;
|
||||
|
||||
virtual ~b3BroadphaseRayCallback() {}
|
||||
};
|
||||
|
||||
#endif //B3_BROADPHASE_CALLBACK_H
|
1325
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp
vendored
Normal file
1325
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp
vendored
Normal file
File diff suppressed because it is too large
Load diff
1269
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
vendored
Normal file
1269
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
vendored
Normal file
File diff suppressed because it is too large
Load diff
804
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
vendored
Normal file
804
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
vendored
Normal file
|
@ -0,0 +1,804 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
///b3DynamicBvhBroadphase implementation by Nathanael Presson
|
||||
|
||||
#include "b3DynamicBvhBroadphase.h"
|
||||
#include "b3OverlappingPair.h"
|
||||
|
||||
//
|
||||
// Profiling
|
||||
//
|
||||
|
||||
#if B3_DBVT_BP_PROFILE||B3_DBVT_BP_ENABLE_BENCHMARK
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
struct b3ProfileScope
|
||||
{
|
||||
__forceinline b3ProfileScope(b3Clock& clock,unsigned long& value) :
|
||||
m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
|
||||
{
|
||||
}
|
||||
__forceinline ~b3ProfileScope()
|
||||
{
|
||||
(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
|
||||
}
|
||||
b3Clock* m_clock;
|
||||
unsigned long* m_value;
|
||||
unsigned long m_base;
|
||||
};
|
||||
#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock,_value_)
|
||||
#else
|
||||
#define b3SPC(_value_)
|
||||
#endif
|
||||
|
||||
//
|
||||
// Helpers
|
||||
//
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline void b3ListAppend(T* item,T*& list)
|
||||
{
|
||||
item->links[0]=0;
|
||||
item->links[1]=list;
|
||||
if(list) list->links[0]=item;
|
||||
list=item;
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline void b3ListRemove(T* item,T*& list)
|
||||
{
|
||||
if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
|
||||
if(item->links[1]) item->links[1]->links[0]=item->links[0];
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline int b3ListCount(T* root)
|
||||
{
|
||||
int n=0;
|
||||
while(root) { ++n;root=root->links[1]; }
|
||||
return(n);
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline void b3Clear(T& value)
|
||||
{
|
||||
static const struct ZeroDummy : T {} zerodummy;
|
||||
value=zerodummy;
|
||||
}
|
||||
|
||||
//
|
||||
// Colliders
|
||||
//
|
||||
|
||||
/* Tree collider */
|
||||
struct b3DbvtTreeCollider : b3DynamicBvh::ICollide
|
||||
{
|
||||
b3DynamicBvhBroadphase* pbp;
|
||||
b3DbvtProxy* proxy;
|
||||
b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
|
||||
void Process(const b3DbvtNode* na,const b3DbvtNode* nb)
|
||||
{
|
||||
if(na!=nb)
|
||||
{
|
||||
b3DbvtProxy* pa=(b3DbvtProxy*)na->data;
|
||||
b3DbvtProxy* pb=(b3DbvtProxy*)nb->data;
|
||||
#if B3_DBVT_BP_SORTPAIRS
|
||||
if(pa->m_uniqueId>pb->m_uniqueId)
|
||||
b3Swap(pa,pb);
|
||||
#endif
|
||||
pbp->m_paircache->addOverlappingPair(pa->getUid(),pb->getUid());
|
||||
++pbp->m_newpairs;
|
||||
}
|
||||
}
|
||||
void Process(const b3DbvtNode* n)
|
||||
{
|
||||
Process(n,proxy->leaf);
|
||||
}
|
||||
};
|
||||
|
||||
//
|
||||
// b3DynamicBvhBroadphase
|
||||
//
|
||||
|
||||
//
|
||||
b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache)
|
||||
{
|
||||
m_deferedcollide = false;
|
||||
m_needcleanup = true;
|
||||
m_releasepaircache = (paircache!=0)?false:true;
|
||||
m_prediction = 0;
|
||||
m_stageCurrent = 0;
|
||||
m_fixedleft = 0;
|
||||
m_fupdates = 1;
|
||||
m_dupdates = 0;
|
||||
m_cupdates = 10;
|
||||
m_newpairs = 1;
|
||||
m_updates_call = 0;
|
||||
m_updates_done = 0;
|
||||
m_updates_ratio = 0;
|
||||
m_paircache = paircache? paircache : new(b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache),16)) b3HashedOverlappingPairCache();
|
||||
|
||||
m_pid = 0;
|
||||
m_cid = 0;
|
||||
for(int i=0;i<=STAGECOUNT;++i)
|
||||
{
|
||||
m_stageRoots[i]=0;
|
||||
}
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
b3Clear(m_profiling);
|
||||
#endif
|
||||
m_proxies.resize(proxyCapacity);
|
||||
}
|
||||
|
||||
//
|
||||
b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
|
||||
{
|
||||
if(m_releasepaircache)
|
||||
{
|
||||
m_paircache->~b3OverlappingPairCache();
|
||||
b3AlignedFree(m_paircache);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
int objectId,
|
||||
void* userPtr,
|
||||
int collisionFilterGroup,
|
||||
int collisionFilterMask)
|
||||
{
|
||||
b3DbvtProxy* mem = &m_proxies[objectId];
|
||||
b3DbvtProxy* proxy=new(mem) b3DbvtProxy( aabbMin,aabbMax,userPtr,
|
||||
collisionFilterGroup,
|
||||
collisionFilterMask);
|
||||
|
||||
b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
|
||||
//bproxy->aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
proxy->stage = m_stageCurrent;
|
||||
proxy->m_uniqueId = objectId;
|
||||
proxy->leaf = m_sets[0].insert(aabb,proxy);
|
||||
b3ListAppend(proxy,m_stageRoots[m_stageCurrent]);
|
||||
if(!m_deferedcollide)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
collider.proxy=proxy;
|
||||
m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
|
||||
m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
|
||||
}
|
||||
return(proxy);
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::destroyProxy( b3BroadphaseProxy* absproxy,
|
||||
b3Dispatcher* dispatcher)
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
if(proxy->stage==STAGECOUNT)
|
||||
m_sets[1].remove(proxy->leaf);
|
||||
else
|
||||
m_sets[0].remove(proxy->leaf);
|
||||
b3ListRemove(proxy,m_stageRoots[proxy->stage]);
|
||||
m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(),dispatcher);
|
||||
|
||||
m_needcleanup=true;
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
|
||||
{
|
||||
const b3DbvtProxy* proxy=&m_proxies[objectId];
|
||||
aabbMin = proxy->m_aabbMin;
|
||||
aabbMax = proxy->m_aabbMax;
|
||||
}
|
||||
/*
|
||||
void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
aabbMin = proxy->m_aabbMin;
|
||||
aabbMax = proxy->m_aabbMax;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
struct BroadphaseRayTester : b3DynamicBvh::ICollide
|
||||
{
|
||||
b3BroadphaseRayCallback& m_rayCallback;
|
||||
BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
|
||||
:m_rayCallback(orgCallback)
|
||||
{
|
||||
}
|
||||
void Process(const b3DbvtNode* leaf)
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data;
|
||||
m_rayCallback.process(proxy);
|
||||
}
|
||||
};
|
||||
|
||||
void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax)
|
||||
{
|
||||
BroadphaseRayTester callback(rayCallback);
|
||||
|
||||
m_sets[0].rayTestInternal( m_sets[0].m_root,
|
||||
rayFrom,
|
||||
rayTo,
|
||||
rayCallback.m_rayDirectionInverse,
|
||||
rayCallback.m_signs,
|
||||
rayCallback.m_lambda_max,
|
||||
aabbMin,
|
||||
aabbMax,
|
||||
callback);
|
||||
|
||||
m_sets[1].rayTestInternal( m_sets[1].m_root,
|
||||
rayFrom,
|
||||
rayTo,
|
||||
rayCallback.m_rayDirectionInverse,
|
||||
rayCallback.m_signs,
|
||||
rayCallback.m_lambda_max,
|
||||
aabbMin,
|
||||
aabbMax,
|
||||
callback);
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct BroadphaseAabbTester : b3DynamicBvh::ICollide
|
||||
{
|
||||
b3BroadphaseAabbCallback& m_aabbCallback;
|
||||
BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
|
||||
:m_aabbCallback(orgCallback)
|
||||
{
|
||||
}
|
||||
void Process(const b3DbvtNode* leaf)
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data;
|
||||
m_aabbCallback.process(proxy);
|
||||
}
|
||||
};
|
||||
|
||||
void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3BroadphaseAabbCallback& aabbCallback)
|
||||
{
|
||||
BroadphaseAabbTester callback(aabbCallback);
|
||||
|
||||
const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds=b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
//process all children, that overlap with the given AABB bounds
|
||||
m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
|
||||
m_sets[1].collideTV(m_sets[1].m_root,bounds,callback);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::setAabb(int objectId,
|
||||
const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
b3DbvtProxy* proxy=&m_proxies[objectId];
|
||||
// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
#if B3_DBVT_BP_PREVENTFALSEUPDATE
|
||||
if(b3NotEqual(aabb,proxy->leaf->volume))
|
||||
#endif
|
||||
{
|
||||
bool docollide=false;
|
||||
if(proxy->stage==STAGECOUNT)
|
||||
{/* fixed -> dynamic set */
|
||||
m_sets[1].remove(proxy->leaf);
|
||||
proxy->leaf=m_sets[0].insert(aabb,proxy);
|
||||
docollide=true;
|
||||
}
|
||||
else
|
||||
{/* dynamic set */
|
||||
++m_updates_call;
|
||||
if(b3Intersect(proxy->leaf->volume,aabb))
|
||||
{/* Moving */
|
||||
|
||||
const b3Vector3 delta=aabbMin-proxy->m_aabbMin;
|
||||
b3Vector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
|
||||
if(delta[0]<0) velocity[0]=-velocity[0];
|
||||
if(delta[1]<0) velocity[1]=-velocity[1];
|
||||
if(delta[2]<0) velocity[2]=-velocity[2];
|
||||
if (
|
||||
#ifdef B3_DBVT_BP_MARGIN
|
||||
m_sets[0].update(proxy->leaf,aabb,velocity,B3_DBVT_BP_MARGIN)
|
||||
#else
|
||||
m_sets[0].update(proxy->leaf,aabb,velocity)
|
||||
#endif
|
||||
)
|
||||
{
|
||||
++m_updates_done;
|
||||
docollide=true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{/* Teleporting */
|
||||
m_sets[0].update(proxy->leaf,aabb);
|
||||
++m_updates_done;
|
||||
docollide=true;
|
||||
}
|
||||
}
|
||||
b3ListRemove(proxy,m_stageRoots[proxy->stage]);
|
||||
proxy->m_aabbMin = aabbMin;
|
||||
proxy->m_aabbMax = aabbMax;
|
||||
proxy->stage = m_stageCurrent;
|
||||
b3ListAppend(proxy,m_stageRoots[m_stageCurrent]);
|
||||
if(docollide)
|
||||
{
|
||||
m_needcleanup=true;
|
||||
if(!m_deferedcollide)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::setAabbForceUpdate( b3BroadphaseProxy* absproxy,
|
||||
const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
bool docollide=false;
|
||||
if(proxy->stage==STAGECOUNT)
|
||||
{/* fixed -> dynamic set */
|
||||
m_sets[1].remove(proxy->leaf);
|
||||
proxy->leaf=m_sets[0].insert(aabb,proxy);
|
||||
docollide=true;
|
||||
}
|
||||
else
|
||||
{/* dynamic set */
|
||||
++m_updates_call;
|
||||
/* Teleporting */
|
||||
m_sets[0].update(proxy->leaf,aabb);
|
||||
++m_updates_done;
|
||||
docollide=true;
|
||||
}
|
||||
b3ListRemove(proxy,m_stageRoots[proxy->stage]);
|
||||
proxy->m_aabbMin = aabbMin;
|
||||
proxy->m_aabbMax = aabbMax;
|
||||
proxy->stage = m_stageCurrent;
|
||||
b3ListAppend(proxy,m_stageRoots[m_stageCurrent]);
|
||||
if(docollide)
|
||||
{
|
||||
m_needcleanup=true;
|
||||
if(!m_deferedcollide)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
collide(dispatcher);
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
if(0==(m_pid%B3_DBVT_BP_PROFILING_RATE))
|
||||
{
|
||||
printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
|
||||
unsigned int total=m_profiling.m_total;
|
||||
if(total<=0) total=1;
|
||||
printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("total: %uus\r\n",total/B3_DBVT_BP_PROFILING_RATE);
|
||||
const unsigned long sum=m_profiling.m_ddcollide+
|
||||
m_profiling.m_fdcollide+
|
||||
m_profiling.m_cleanup;
|
||||
printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*B3_DBVT_BP_PROFILING_RATE));
|
||||
b3Clear(m_profiling);
|
||||
m_clock.reset();
|
||||
}
|
||||
#endif
|
||||
|
||||
performDeferredRemoval(dispatcher);
|
||||
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
if (m_paircache->hasDeferredRemoval())
|
||||
{
|
||||
|
||||
b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
|
||||
|
||||
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
|
||||
|
||||
int invalidPair = 0;
|
||||
|
||||
|
||||
int i;
|
||||
|
||||
b3BroadphasePair previousPair = b3MakeBroadphasePair(-1,-1);
|
||||
|
||||
|
||||
|
||||
for (i=0;i<overlappingPairArray.size();i++)
|
||||
{
|
||||
|
||||
b3BroadphasePair& pair = overlappingPairArray[i];
|
||||
|
||||
bool isDuplicate = (pair == previousPair);
|
||||
|
||||
previousPair = pair;
|
||||
|
||||
bool needsRemoval = false;
|
||||
|
||||
if (!isDuplicate)
|
||||
{
|
||||
//important to perform AABB check that is consistent with the broadphase
|
||||
b3DbvtProxy* pa=&m_proxies[pair.x];
|
||||
b3DbvtProxy* pb=&m_proxies[pair.y];
|
||||
bool hasOverlap = b3Intersect(pa->leaf->volume,pb->leaf->volume);
|
||||
|
||||
if (hasOverlap)
|
||||
{
|
||||
needsRemoval = false;
|
||||
} else
|
||||
{
|
||||
needsRemoval = true;
|
||||
}
|
||||
} else
|
||||
{
|
||||
//remove duplicate
|
||||
needsRemoval = true;
|
||||
//should have no algorithm
|
||||
}
|
||||
|
||||
if (needsRemoval)
|
||||
{
|
||||
m_paircache->cleanOverlappingPair(pair,dispatcher);
|
||||
|
||||
pair.x = -1;
|
||||
pair.y = -1;
|
||||
invalidPair++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//perform a sort, to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
|
||||
overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
|
||||
{
|
||||
/*printf("---------------------------------------------------------\n");
|
||||
printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
|
||||
printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
|
||||
printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
|
||||
{
|
||||
printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
|
||||
getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
b3SPC(m_profiling.m_total);
|
||||
/* optimize */
|
||||
m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
|
||||
if(m_fixedleft)
|
||||
{
|
||||
const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
|
||||
m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
|
||||
m_fixedleft=b3Max<int>(0,m_fixedleft-count);
|
||||
}
|
||||
/* dynamic -> fixed set */
|
||||
m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
|
||||
b3DbvtProxy* current=m_stageRoots[m_stageCurrent];
|
||||
if(current)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
do {
|
||||
b3DbvtProxy* next=current->links[1];
|
||||
b3ListRemove(current,m_stageRoots[current->stage]);
|
||||
b3ListAppend(current,m_stageRoots[STAGECOUNT]);
|
||||
#if B3_DBVT_BP_ACCURATESLEEPING
|
||||
m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
|
||||
collider.proxy=current;
|
||||
b3DynamicBvh::collideTV(m_sets[0].m_root,current->aabb,collider);
|
||||
b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider);
|
||||
#endif
|
||||
m_sets[0].remove(current->leaf);
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) curAabb=b3DbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
|
||||
current->leaf = m_sets[1].insert(curAabb,current);
|
||||
current->stage = STAGECOUNT;
|
||||
current = next;
|
||||
} while(current);
|
||||
m_fixedleft=m_sets[1].m_leaves;
|
||||
m_needcleanup=true;
|
||||
}
|
||||
/* collide dynamics */
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
if(m_deferedcollide)
|
||||
{
|
||||
b3SPC(m_profiling.m_fdcollide);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
|
||||
}
|
||||
if(m_deferedcollide)
|
||||
{
|
||||
b3SPC(m_profiling.m_ddcollide);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
|
||||
}
|
||||
}
|
||||
/* clean up */
|
||||
if(m_needcleanup)
|
||||
{
|
||||
b3SPC(m_profiling.m_cleanup);
|
||||
b3BroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
|
||||
if(pairs.size()>0)
|
||||
{
|
||||
|
||||
int ni=b3Min(pairs.size(),b3Max<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
|
||||
for(int i=0;i<ni;++i)
|
||||
{
|
||||
b3BroadphasePair& p=pairs[(m_cid+i)%pairs.size()];
|
||||
b3DbvtProxy* pa=&m_proxies[p.x];
|
||||
b3DbvtProxy* pb=&m_proxies[p.y];
|
||||
if(!b3Intersect(pa->leaf->volume,pb->leaf->volume))
|
||||
{
|
||||
#if B3_DBVT_BP_SORTPAIRS
|
||||
if(pa->m_uniqueId>pb->m_uniqueId)
|
||||
b3Swap(pa,pb);
|
||||
#endif
|
||||
m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),dispatcher);
|
||||
--ni;--i;
|
||||
}
|
||||
}
|
||||
if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
|
||||
}
|
||||
}
|
||||
++m_pid;
|
||||
m_newpairs=1;
|
||||
m_needcleanup=false;
|
||||
if(m_updates_call>0)
|
||||
{ m_updates_ratio=m_updates_done/(b3Scalar)m_updates_call; }
|
||||
else
|
||||
{ m_updates_ratio=0; }
|
||||
m_updates_done/=2;
|
||||
m_updates_call/=2;
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::optimize()
|
||||
{
|
||||
m_sets[0].optimizeTopDown();
|
||||
m_sets[1].optimizeTopDown();
|
||||
}
|
||||
|
||||
//
|
||||
b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
|
||||
{
|
||||
return(m_paircache);
|
||||
}
|
||||
|
||||
//
|
||||
const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
|
||||
{
|
||||
return(m_paircache);
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const
|
||||
{
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds;
|
||||
|
||||
if(!m_sets[0].empty())
|
||||
if(!m_sets[1].empty()) b3Merge( m_sets[0].m_root->volume,
|
||||
m_sets[1].m_root->volume,bounds);
|
||||
else
|
||||
bounds=m_sets[0].m_root->volume;
|
||||
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
|
||||
else
|
||||
bounds=b3DbvtVolume::FromCR(b3MakeVector3(0,0,0),0);
|
||||
aabbMin=bounds.Mins();
|
||||
aabbMax=bounds.Maxs();
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
|
||||
if (!totalObjects)
|
||||
{
|
||||
//reset internal dynamic tree data structures
|
||||
m_sets[0].clear();
|
||||
m_sets[1].clear();
|
||||
|
||||
m_deferedcollide = false;
|
||||
m_needcleanup = true;
|
||||
m_stageCurrent = 0;
|
||||
m_fixedleft = 0;
|
||||
m_fupdates = 1;
|
||||
m_dupdates = 0;
|
||||
m_cupdates = 10;
|
||||
m_newpairs = 1;
|
||||
m_updates_call = 0;
|
||||
m_updates_done = 0;
|
||||
m_updates_ratio = 0;
|
||||
|
||||
m_pid = 0;
|
||||
m_cid = 0;
|
||||
for(int i=0;i<=STAGECOUNT;++i)
|
||||
{
|
||||
m_stageRoots[i]=0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::printStats()
|
||||
{}
|
||||
|
||||
//
|
||||
#if B3_DBVT_BP_ENABLE_BENCHMARK
|
||||
|
||||
struct b3BroadphaseBenchmark
|
||||
{
|
||||
struct Experiment
|
||||
{
|
||||
const char* name;
|
||||
int object_count;
|
||||
int update_count;
|
||||
int spawn_count;
|
||||
int iterations;
|
||||
b3Scalar speed;
|
||||
b3Scalar amplitude;
|
||||
};
|
||||
struct Object
|
||||
{
|
||||
b3Vector3 center;
|
||||
b3Vector3 extents;
|
||||
b3BroadphaseProxy* proxy;
|
||||
b3Scalar time;
|
||||
void update(b3Scalar speed,b3Scalar amplitude,b3BroadphaseInterface* pbi)
|
||||
{
|
||||
time += speed;
|
||||
center[0] = b3Cos(time*(b3Scalar)2.17)*amplitude+
|
||||
b3Sin(time)*amplitude/2;
|
||||
center[1] = b3Cos(time*(b3Scalar)1.38)*amplitude+
|
||||
b3Sin(time)*amplitude;
|
||||
center[2] = b3Sin(time*(b3Scalar)0.777)*amplitude;
|
||||
pbi->setAabb(proxy,center-extents,center+extents,0);
|
||||
}
|
||||
};
|
||||
static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); }
|
||||
static b3Scalar UnitRand() { return(UnsignedRand(16384)/(b3Scalar)16384); }
|
||||
static void OutputTime(const char* name,b3Clock& c,unsigned count=0)
|
||||
{
|
||||
const unsigned long us=c.getTimeMicroseconds();
|
||||
const unsigned long ms=(us+500)/1000;
|
||||
const b3Scalar sec=us/(b3Scalar)(1000*1000);
|
||||
if(count>0)
|
||||
printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
|
||||
else
|
||||
printf("%s : %u us (%u ms)\r\n",name,us,ms);
|
||||
}
|
||||
};
|
||||
|
||||
void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
|
||||
{
|
||||
static const b3BroadphaseBenchmark::Experiment experiments[]=
|
||||
{
|
||||
{"1024o.10%",1024,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
|
||||
/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
|
||||
{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
|
||||
};
|
||||
static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]);
|
||||
b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
|
||||
b3Clock wallclock;
|
||||
/* Begin */
|
||||
for(int iexp=0;iexp<nexperiments;++iexp)
|
||||
{
|
||||
const b3BroadphaseBenchmark::Experiment& experiment=experiments[iexp];
|
||||
const int object_count=experiment.object_count;
|
||||
const int update_count=(object_count*experiment.update_count)/100;
|
||||
const int spawn_count=(object_count*experiment.spawn_count)/100;
|
||||
const b3Scalar speed=experiment.speed;
|
||||
const b3Scalar amplitude=experiment.amplitude;
|
||||
printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
|
||||
printf("\tObjects: %u\r\n",object_count);
|
||||
printf("\tUpdate: %u\r\n",update_count);
|
||||
printf("\tSpawn: %u\r\n",spawn_count);
|
||||
printf("\tSpeed: %f\r\n",speed);
|
||||
printf("\tAmplitude: %f\r\n",amplitude);
|
||||
srand(180673);
|
||||
/* Create objects */
|
||||
wallclock.reset();
|
||||
objects.reserve(object_count);
|
||||
for(int i=0;i<object_count;++i)
|
||||
{
|
||||
b3BroadphaseBenchmark::Object* po=new b3BroadphaseBenchmark::Object();
|
||||
po->center[0]=b3BroadphaseBenchmark::UnitRand()*50;
|
||||
po->center[1]=b3BroadphaseBenchmark::UnitRand()*50;
|
||||
po->center[2]=b3BroadphaseBenchmark::UnitRand()*50;
|
||||
po->extents[0]=b3BroadphaseBenchmark::UnitRand()*2+2;
|
||||
po->extents[1]=b3BroadphaseBenchmark::UnitRand()*2+2;
|
||||
po->extents[2]=b3BroadphaseBenchmark::UnitRand()*2+2;
|
||||
po->time=b3BroadphaseBenchmark::UnitRand()*2000;
|
||||
po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
|
||||
objects.push_back(po);
|
||||
}
|
||||
b3BroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
|
||||
/* First update */
|
||||
wallclock.reset();
|
||||
for(int i=0;i<objects.size();++i)
|
||||
{
|
||||
objects[i]->update(speed,amplitude,pbi);
|
||||
}
|
||||
b3BroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
|
||||
/* Updates */
|
||||
wallclock.reset();
|
||||
for(int i=0;i<experiment.iterations;++i)
|
||||
{
|
||||
for(int j=0;j<update_count;++j)
|
||||
{
|
||||
objects[j]->update(speed,amplitude,pbi);
|
||||
}
|
||||
pbi->calculateOverlappingPairs(0);
|
||||
}
|
||||
b3BroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
|
||||
/* Clean up */
|
||||
wallclock.reset();
|
||||
for(int i=0;i<objects.size();++i)
|
||||
{
|
||||
pbi->destroyProxy(objects[i]->proxy,0);
|
||||
delete objects[i];
|
||||
}
|
||||
objects.resize(0);
|
||||
b3BroadphaseBenchmark::OutputTime("\tRelease",wallclock);
|
||||
}
|
||||
|
||||
}
|
||||
#else
|
||||
/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
|
||||
{}
|
||||
*/
|
||||
#endif
|
||||
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
#undef b3SPC
|
||||
#endif
|
||||
|
206
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
vendored
Normal file
206
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
vendored
Normal file
|
@ -0,0 +1,206 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
///b3DynamicBvhBroadphase implementation by Nathanael Presson
|
||||
#ifndef B3_DBVT_BROADPHASE_H
|
||||
#define B3_DBVT_BROADPHASE_H
|
||||
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
#include "b3BroadphaseCallback.h"
|
||||
|
||||
//
|
||||
// Compile time config
|
||||
//
|
||||
|
||||
#define B3_DBVT_BP_PROFILE 0
|
||||
//#define B3_DBVT_BP_SORTPAIRS 1
|
||||
#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
|
||||
#define B3_DBVT_BP_ACCURATESLEEPING 0
|
||||
#define B3_DBVT_BP_ENABLE_BENCHMARK 0
|
||||
#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
|
||||
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
#define B3_DBVT_BP_PROFILING_RATE 256
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy
|
||||
{
|
||||
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///optional filtering to cull potential collisions
|
||||
enum CollisionFilterGroups
|
||||
{
|
||||
DefaultFilter = 1,
|
||||
StaticFilter = 2,
|
||||
KinematicFilter = 4,
|
||||
DebrisFilter = 8,
|
||||
SensorTrigger = 16,
|
||||
CharacterFilter = 32,
|
||||
AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
|
||||
};
|
||||
|
||||
//Usually the client b3CollisionObject or Rigidbody class
|
||||
void* m_clientObject;
|
||||
int m_collisionFilterGroup;
|
||||
int m_collisionFilterMask;
|
||||
int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
|
||||
|
||||
b3Vector3 m_aabbMin;
|
||||
b3Vector3 m_aabbMax;
|
||||
|
||||
B3_FORCE_INLINE int getUid() const
|
||||
{
|
||||
return m_uniqueId;
|
||||
}
|
||||
|
||||
//used for memory pools
|
||||
b3BroadphaseProxy() :m_clientObject(0)
|
||||
{
|
||||
}
|
||||
|
||||
b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask)
|
||||
:m_clientObject(userPtr),
|
||||
m_collisionFilterGroup(collisionFilterGroup),
|
||||
m_collisionFilterMask(collisionFilterMask),
|
||||
m_aabbMin(aabbMin),
|
||||
m_aabbMax(aabbMax)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
// b3DbvtProxy
|
||||
//
|
||||
struct b3DbvtProxy : b3BroadphaseProxy
|
||||
{
|
||||
/* Fields */
|
||||
//b3DbvtAabbMm aabb;
|
||||
b3DbvtNode* leaf;
|
||||
b3DbvtProxy* links[2];
|
||||
int stage;
|
||||
/* ctor */
|
||||
|
||||
explicit b3DbvtProxy() {}
|
||||
b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) :
|
||||
b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
|
||||
{
|
||||
links[0]=links[1]=0;
|
||||
}
|
||||
};
|
||||
|
||||
typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
|
||||
|
||||
///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
|
||||
///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
|
||||
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
|
||||
struct b3DynamicBvhBroadphase
|
||||
{
|
||||
/* Config */
|
||||
enum {
|
||||
DYNAMIC_SET = 0, /* Dynamic set index */
|
||||
FIXED_SET = 1, /* Fixed set index */
|
||||
STAGECOUNT = 2 /* Number of stages */
|
||||
};
|
||||
/* Fields */
|
||||
b3DynamicBvh m_sets[2]; // Dbvt sets
|
||||
b3DbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
|
||||
|
||||
b3AlignedObjectArray<b3DbvtProxy> m_proxies;
|
||||
b3OverlappingPairCache* m_paircache; // Pair cache
|
||||
b3Scalar m_prediction; // Velocity prediction
|
||||
int m_stageCurrent; // Current stage
|
||||
int m_fupdates; // % of fixed updates per frame
|
||||
int m_dupdates; // % of dynamic updates per frame
|
||||
int m_cupdates; // % of cleanup updates per frame
|
||||
int m_newpairs; // Number of pairs created
|
||||
int m_fixedleft; // Fixed optimization left
|
||||
unsigned m_updates_call; // Number of updates call
|
||||
unsigned m_updates_done; // Number of updates done
|
||||
b3Scalar m_updates_ratio; // m_updates_done/m_updates_call
|
||||
int m_pid; // Parse id
|
||||
int m_cid; // Cleanup index
|
||||
bool m_releasepaircache; // Release pair cache on delete
|
||||
bool m_deferedcollide; // Defere dynamic/static collision to collide call
|
||||
bool m_needcleanup; // Need to run cleanup?
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
b3Clock m_clock;
|
||||
struct {
|
||||
unsigned long m_total;
|
||||
unsigned long m_ddcollide;
|
||||
unsigned long m_fdcollide;
|
||||
unsigned long m_cleanup;
|
||||
unsigned long m_jobcount;
|
||||
} m_profiling;
|
||||
#endif
|
||||
/* Methods */
|
||||
b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0);
|
||||
virtual ~b3DynamicBvhBroadphase();
|
||||
void collide(b3Dispatcher* dispatcher);
|
||||
void optimize();
|
||||
|
||||
/* b3BroadphaseInterface Implementation */
|
||||
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr, int collisionFilterGroup, int collisionFilterMask);
|
||||
virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
|
||||
virtual void setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
|
||||
virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
|
||||
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
|
||||
|
||||
//virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
|
||||
virtual void getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
|
||||
virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
|
||||
virtual b3OverlappingPairCache* getOverlappingPairCache();
|
||||
virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
|
||||
virtual void getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const;
|
||||
virtual void printStats();
|
||||
|
||||
|
||||
///reset broadphase internal structures, to ensure determinism/reproducability
|
||||
virtual void resetPool(b3Dispatcher* dispatcher);
|
||||
|
||||
void performDeferredRemoval(b3Dispatcher* dispatcher);
|
||||
|
||||
void setVelocityPrediction(b3Scalar prediction)
|
||||
{
|
||||
m_prediction = prediction;
|
||||
}
|
||||
b3Scalar getVelocityPrediction() const
|
||||
{
|
||||
return m_prediction;
|
||||
}
|
||||
|
||||
///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
|
||||
///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
|
||||
///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
|
||||
///http://code.google.com/p/bullet/issues/detail?id=223
|
||||
void setAabbForceUpdate( b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/);
|
||||
|
||||
//static void benchmark(b3BroadphaseInterface*);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif
|
72
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h
vendored
Normal file
72
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h
vendored
Normal file
|
@ -0,0 +1,72 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_OVERLAPPING_PAIR_H
|
||||
#define B3_OVERLAPPING_PAIR_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
|
||||
#define B3_NEW_PAIR_MARKER -1
|
||||
#define B3_REMOVED_PAIR_MARKER -2
|
||||
|
||||
typedef b3Int4 b3BroadphasePair;
|
||||
|
||||
inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
|
||||
{
|
||||
b3Int4 pair;
|
||||
|
||||
if (xx < yy)
|
||||
{
|
||||
pair.x = xx;
|
||||
pair.y = yy;
|
||||
}
|
||||
else
|
||||
{
|
||||
pair.x = yy;
|
||||
pair.y = xx;
|
||||
}
|
||||
pair.z = B3_NEW_PAIR_MARKER;
|
||||
pair.w = B3_NEW_PAIR_MARKER;
|
||||
return pair;
|
||||
}
|
||||
|
||||
/*struct b3BroadphasePair : public b3Int4
|
||||
{
|
||||
explicit b3BroadphasePair(){}
|
||||
|
||||
};
|
||||
*/
|
||||
|
||||
class b3BroadphasePairSortPredicate
|
||||
{
|
||||
public:
|
||||
|
||||
bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const
|
||||
{
|
||||
const int uidA0 = a.x;
|
||||
const int uidB0 = b.x;
|
||||
const int uidA1 = a.y;
|
||||
const int uidB1 = b.y;
|
||||
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
|
||||
}
|
||||
};
|
||||
|
||||
B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
|
||||
{
|
||||
return (a.x == b.x ) && (a.y == b.y );
|
||||
}
|
||||
|
||||
#endif //B3_OVERLAPPING_PAIR_H
|
||||
|
638
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp
vendored
Normal file
638
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp
vendored
Normal file
|
@ -0,0 +1,638 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "b3OverlappingPairCache.h"
|
||||
|
||||
//#include "b3Dispatcher.h"
|
||||
//#include "b3CollisionAlgorithm.h"
|
||||
#include "Bullet3Geometry/b3AabbUtil.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
int b3g_overlappingPairs = 0;
|
||||
int b3g_removePairs =0;
|
||||
int b3g_addedPairs =0;
|
||||
int b3g_findPairs =0;
|
||||
|
||||
|
||||
|
||||
|
||||
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
|
||||
m_overlapFilterCallback(0)
|
||||
//, m_blockedForChanges(false)
|
||||
{
|
||||
int initialAllocatedSize= 2;
|
||||
m_overlappingPairArray.reserve(initialAllocatedSize);
|
||||
growTables();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
|
||||
{
|
||||
/* if (pair.m_algorithm)
|
||||
{
|
||||
{
|
||||
pair.m_algorithm->~b3CollisionAlgorithm();
|
||||
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
|
||||
pair.m_algorithm=0;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
class CleanPairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_cleanProxy;
|
||||
b3OverlappingPairCache* m_pairCache;
|
||||
b3Dispatcher* m_dispatcher;
|
||||
|
||||
public:
|
||||
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
|
||||
:m_cleanProxy(cleanProxy),
|
||||
m_pairCache(pairCache),
|
||||
m_dispatcher(dispatcher)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
if ((pair.x == m_cleanProxy) ||
|
||||
(pair.y == m_cleanProxy))
|
||||
{
|
||||
m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
CleanPairCallback cleanPairs(proxy,this,dispatcher);
|
||||
|
||||
processAllOverlappingPairs(&cleanPairs,dispatcher);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
class RemovePairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_obsoleteProxy;
|
||||
|
||||
public:
|
||||
RemovePairCallback(int obsoleteProxy)
|
||||
:m_obsoleteProxy(obsoleteProxy)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
return ((pair.x == m_obsoleteProxy) ||
|
||||
(pair.y == m_obsoleteProxy));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
RemovePairCallback removeCallback(proxy);
|
||||
|
||||
processAllOverlappingPairs(&removeCallback,dispatcher);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
|
||||
{
|
||||
b3g_findPairs++;
|
||||
if(proxy0 >proxy1)
|
||||
b3Swap(proxy0,proxy1);
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
|
||||
|
||||
if (hash >= m_hashTable.size())
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int index = m_hashTable[hash];
|
||||
while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
|
||||
{
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (index == B3_NULL_PAIR)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
b3Assert(index < m_overlappingPairArray.size());
|
||||
|
||||
return &m_overlappingPairArray[index];
|
||||
}
|
||||
|
||||
//#include <stdio.h>
|
||||
|
||||
void b3HashedOverlappingPairCache::growTables()
|
||||
{
|
||||
|
||||
int newCapacity = m_overlappingPairArray.capacity();
|
||||
|
||||
if (m_hashTable.size() < newCapacity)
|
||||
{
|
||||
//grow hashtable and next table
|
||||
int curHashtableSize = m_hashTable.size();
|
||||
|
||||
m_hashTable.resize(newCapacity);
|
||||
m_next.resize(newCapacity);
|
||||
|
||||
|
||||
int i;
|
||||
|
||||
for (i= 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_hashTable[i] = B3_NULL_PAIR;
|
||||
}
|
||||
for (i = 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_next[i] = B3_NULL_PAIR;
|
||||
}
|
||||
|
||||
for(i=0;i<curHashtableSize;i++)
|
||||
{
|
||||
|
||||
const b3BroadphasePair& pair = m_overlappingPairArray[i];
|
||||
int proxyId1 = pair.x;
|
||||
int proxyId2 = pair.y;
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
|
||||
m_next[i] = m_hashTable[hashValue];
|
||||
m_hashTable[hashValue] = i;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
|
||||
{
|
||||
if(proxy0>proxy1)
|
||||
b3Swap(proxy0,proxy1);
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
|
||||
|
||||
|
||||
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
|
||||
if (pair != NULL)
|
||||
{
|
||||
return pair;
|
||||
}
|
||||
/*for(int i=0;i<m_overlappingPairArray.size();++i)
|
||||
{
|
||||
if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
|
||||
(m_overlappingPairArray[i].m_pProxy1==proxy1))
|
||||
{
|
||||
printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
|
||||
internalFindPair(proxy0, proxy1, hash);
|
||||
}
|
||||
}*/
|
||||
int count = m_overlappingPairArray.size();
|
||||
int oldCapacity = m_overlappingPairArray.capacity();
|
||||
pair = &m_overlappingPairArray.expandNonInitializing();
|
||||
|
||||
//this is where we add an actual pair, so also call the 'ghost'
|
||||
// if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
|
||||
|
||||
int newCapacity = m_overlappingPairArray.capacity();
|
||||
|
||||
if (oldCapacity < newCapacity)
|
||||
{
|
||||
growTables();
|
||||
//hash with new capacity
|
||||
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
|
||||
}
|
||||
|
||||
*pair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
|
||||
// pair->m_pProxy0 = proxy0;
|
||||
// pair->m_pProxy1 = proxy1;
|
||||
//pair->m_algorithm = 0;
|
||||
//pair->m_internalTmpValue = 0;
|
||||
|
||||
|
||||
m_next[count] = m_hashTable[hash];
|
||||
m_hashTable[hash] = count;
|
||||
|
||||
return pair;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher)
|
||||
{
|
||||
b3g_removePairs++;
|
||||
if(proxy0>proxy1)
|
||||
b3Swap(proxy0,proxy1);
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
|
||||
|
||||
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
|
||||
if (pair == NULL)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
cleanOverlappingPair(*pair,dispatcher);
|
||||
|
||||
|
||||
|
||||
int pairIndex = int(pair - &m_overlappingPairArray[0]);
|
||||
b3Assert(pairIndex < m_overlappingPairArray.size());
|
||||
|
||||
// Remove the pair from the hash table.
|
||||
int index = m_hashTable[hash];
|
||||
b3Assert(index != B3_NULL_PAIR);
|
||||
|
||||
int previous = B3_NULL_PAIR;
|
||||
while (index != pairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_NULL_PAIR)
|
||||
{
|
||||
b3Assert(m_next[previous] == pairIndex);
|
||||
m_next[previous] = m_next[pairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[hash] = m_next[pairIndex];
|
||||
}
|
||||
|
||||
// We now move the last pair into spot of the
|
||||
// pair being removed. We need to fix the hash
|
||||
// table indices to support the move.
|
||||
|
||||
int lastPairIndex = m_overlappingPairArray.size() - 1;
|
||||
|
||||
//if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
|
||||
|
||||
// If the removed pair is the last pair, we are done.
|
||||
if (lastPairIndex == pairIndex)
|
||||
{
|
||||
m_overlappingPairArray.pop_back();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Remove the last pair from the hash table.
|
||||
const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
|
||||
/* missing swap here too, Nat. */
|
||||
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity()-1));
|
||||
|
||||
index = m_hashTable[lastHash];
|
||||
b3Assert(index != B3_NULL_PAIR);
|
||||
|
||||
previous = B3_NULL_PAIR;
|
||||
while (index != lastPairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_NULL_PAIR)
|
||||
{
|
||||
b3Assert(m_next[previous] == lastPairIndex);
|
||||
m_next[previous] = m_next[lastPairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[lastHash] = m_next[lastPairIndex];
|
||||
}
|
||||
|
||||
// Copy the last pair into the remove pair's spot.
|
||||
m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
|
||||
|
||||
// Insert the last pair into the hash table
|
||||
m_next[pairIndex] = m_hashTable[lastHash];
|
||||
m_hashTable[lastHash] = pairIndex;
|
||||
|
||||
m_overlappingPairArray.pop_back();
|
||||
|
||||
return 0;
|
||||
}
|
||||
//#include <stdio.h>
|
||||
|
||||
void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
int i;
|
||||
|
||||
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
|
||||
for (i=0;i<m_overlappingPairArray.size();)
|
||||
{
|
||||
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray[i];
|
||||
if (callback->processOverlap(*pair))
|
||||
{
|
||||
removeOverlappingPair(pair->x,pair->y,dispatcher);
|
||||
|
||||
b3g_overlappingPairs--;
|
||||
} else
|
||||
{
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
///need to keep hashmap in sync with pair address, so rebuild all
|
||||
b3BroadphasePairArray tmpPairs;
|
||||
int i;
|
||||
for (i=0;i<m_overlappingPairArray.size();i++)
|
||||
{
|
||||
tmpPairs.push_back(m_overlappingPairArray[i]);
|
||||
}
|
||||
|
||||
for (i=0;i<tmpPairs.size();i++)
|
||||
{
|
||||
removeOverlappingPair(tmpPairs[i].x,tmpPairs[i].y,dispatcher);
|
||||
}
|
||||
|
||||
for (i = 0; i < m_next.size(); i++)
|
||||
{
|
||||
m_next[i] = B3_NULL_PAIR;
|
||||
}
|
||||
|
||||
tmpPairs.quickSort(b3BroadphasePairSortPredicate());
|
||||
|
||||
for (i=0;i<tmpPairs.size();i++)
|
||||
{
|
||||
addOverlappingPair(tmpPairs[i].x ,tmpPairs[i].y);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, b3Dispatcher* dispatcher )
|
||||
{
|
||||
if (!hasDeferredRemoval())
|
||||
{
|
||||
b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
|
||||
|
||||
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
|
||||
if (findIndex < m_overlappingPairArray.size())
|
||||
{
|
||||
b3g_overlappingPairs--;
|
||||
b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
|
||||
|
||||
cleanOverlappingPair(pair,dispatcher);
|
||||
//if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
|
||||
|
||||
m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
|
||||
m_overlappingPairArray.pop_back();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1)
|
||||
{
|
||||
//don't add overlap with own
|
||||
b3Assert(proxy0 != proxy1);
|
||||
|
||||
if (!needsBroadphaseCollision(proxy0,proxy1))
|
||||
return 0;
|
||||
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
|
||||
*pair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
|
||||
|
||||
b3g_overlappingPairs++;
|
||||
b3g_addedPairs++;
|
||||
|
||||
// if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
|
||||
return pair;
|
||||
|
||||
}
|
||||
|
||||
///this findPair becomes really slow. Either sort the list to speedup the query, or
|
||||
///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
|
||||
///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
|
||||
///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
|
||||
b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1)
|
||||
{
|
||||
if (!needsBroadphaseCollision(proxy0,proxy1))
|
||||
return 0;
|
||||
|
||||
b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1);
|
||||
int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
|
||||
|
||||
if (findIndex < m_overlappingPairArray.size())
|
||||
{
|
||||
//b3Assert(it != m_overlappingPairSet.end());
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
|
||||
return pair;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//#include <stdio.h>
|
||||
|
||||
void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
int i;
|
||||
|
||||
for (i=0;i<m_overlappingPairArray.size();)
|
||||
{
|
||||
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray[i];
|
||||
if (callback->processOverlap(*pair))
|
||||
{
|
||||
cleanOverlappingPair(*pair,dispatcher);
|
||||
pair->x = -1;
|
||||
pair->y = -1;
|
||||
m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
|
||||
m_overlappingPairArray.pop_back();
|
||||
b3g_overlappingPairs--;
|
||||
} else
|
||||
{
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache():
|
||||
m_blockedForChanges(false),
|
||||
m_hasDeferredRemoval(true),
|
||||
m_overlapFilterCallback(0)
|
||||
|
||||
{
|
||||
int initialAllocatedSize= 2;
|
||||
m_overlappingPairArray.reserve(initialAllocatedSize);
|
||||
}
|
||||
|
||||
b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
|
||||
{
|
||||
}
|
||||
|
||||
void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
|
||||
{
|
||||
/* if (pair.m_algorithm)
|
||||
{
|
||||
{
|
||||
pair.m_algorithm->~b3CollisionAlgorithm();
|
||||
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
|
||||
pair.m_algorithm=0;
|
||||
b3g_removePairs--;
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
class CleanPairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_cleanProxy;
|
||||
b3OverlappingPairCache* m_pairCache;
|
||||
b3Dispatcher* m_dispatcher;
|
||||
|
||||
public:
|
||||
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
|
||||
:m_cleanProxy(cleanProxy),
|
||||
m_pairCache(pairCache),
|
||||
m_dispatcher(dispatcher)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
if ((pair.x == m_cleanProxy) ||
|
||||
(pair.y == m_cleanProxy))
|
||||
{
|
||||
m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
CleanPairCallback cleanPairs(proxy,this,dispatcher);
|
||||
|
||||
processAllOverlappingPairs(&cleanPairs,dispatcher);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
|
||||
{
|
||||
|
||||
class RemovePairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_obsoleteProxy;
|
||||
|
||||
public:
|
||||
RemovePairCallback(int obsoleteProxy)
|
||||
:m_obsoleteProxy(obsoleteProxy)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
return ((pair.x == m_obsoleteProxy) ||
|
||||
(pair.y == m_obsoleteProxy));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
RemovePairCallback removeCallback(proxy);
|
||||
|
||||
processAllOverlappingPairs(&removeCallback,dispatcher);
|
||||
}
|
||||
|
||||
void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
//should already be sorted
|
||||
}
|
||||
|
474
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h
vendored
Normal file
474
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h
vendored
Normal file
|
@ -0,0 +1,474 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_OVERLAPPING_PAIR_CACHE_H
|
||||
#define B3_OVERLAPPING_PAIR_CACHE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Int2.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
class b3Dispatcher;
|
||||
#include "b3OverlappingPair.h"
|
||||
|
||||
|
||||
|
||||
typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
|
||||
|
||||
struct b3OverlapCallback
|
||||
{
|
||||
virtual ~b3OverlapCallback()
|
||||
{}
|
||||
//return true for deletion of the pair
|
||||
virtual bool processOverlap(b3BroadphasePair& pair) = 0;
|
||||
|
||||
};
|
||||
|
||||
struct b3OverlapFilterCallback
|
||||
{
|
||||
virtual ~b3OverlapFilterCallback()
|
||||
{}
|
||||
// return true when pairs need collision
|
||||
virtual bool needBroadphaseCollision(int proxy0,int proxy1) const = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
extern int b3g_removePairs;
|
||||
extern int b3g_addedPairs;
|
||||
extern int b3g_findPairs;
|
||||
|
||||
const int B3_NULL_PAIR=0xffffffff;
|
||||
|
||||
///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
|
||||
///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
|
||||
class b3OverlappingPairCache
|
||||
{
|
||||
public:
|
||||
virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
|
||||
|
||||
virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
|
||||
|
||||
virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
|
||||
|
||||
virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
|
||||
|
||||
virtual void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
virtual int getNumOverlappingPairs() const = 0;
|
||||
|
||||
virtual void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
|
||||
|
||||
virtual bool hasDeferredRemoval() = 0;
|
||||
|
||||
//virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
|
||||
|
||||
virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0;
|
||||
virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0;
|
||||
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0;
|
||||
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
|
||||
class b3HashedOverlappingPairCache : public b3OverlappingPairCache
|
||||
{
|
||||
b3BroadphasePairArray m_overlappingPairArray;
|
||||
b3OverlapFilterCallback* m_overlapFilterCallback;
|
||||
// bool m_blockedForChanges;
|
||||
|
||||
|
||||
public:
|
||||
b3HashedOverlappingPairCache();
|
||||
virtual ~b3HashedOverlappingPairCache();
|
||||
|
||||
|
||||
virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
|
||||
|
||||
virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
|
||||
|
||||
B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const
|
||||
{
|
||||
if (m_overlapFilterCallback)
|
||||
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
|
||||
|
||||
bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
|
||||
//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
|
||||
|
||||
return collides;
|
||||
}
|
||||
|
||||
// Add a pair and return the new pair. If the pair already exists,
|
||||
// no new pair is created and the old one is returned.
|
||||
virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)
|
||||
{
|
||||
b3g_addedPairs++;
|
||||
|
||||
if (!needsBroadphaseCollision(proxy0,proxy1))
|
||||
return 0;
|
||||
|
||||
return internalAddPair(proxy0,proxy1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
|
||||
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
|
||||
|
||||
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
const b3BroadphasePair* getOverlappingPairArrayPtr() const
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
b3BroadphasePairArray& getOverlappingPairArray()
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
const b3BroadphasePairArray& getOverlappingPairArray() const
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
|
||||
|
||||
|
||||
|
||||
b3BroadphasePair* findPair(int proxy0, int proxy1);
|
||||
|
||||
int GetCount() const { return m_overlappingPairArray.size(); }
|
||||
// b3BroadphasePair* GetPairs() { return m_pairs; }
|
||||
|
||||
b3OverlapFilterCallback* getOverlapFilterCallback()
|
||||
{
|
||||
return m_overlapFilterCallback;
|
||||
}
|
||||
|
||||
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
|
||||
{
|
||||
m_overlapFilterCallback = callback;
|
||||
}
|
||||
|
||||
int getNumOverlappingPairs() const
|
||||
{
|
||||
return m_overlappingPairArray.size();
|
||||
}
|
||||
private:
|
||||
|
||||
b3BroadphasePair* internalAddPair(int proxy0,int proxy1);
|
||||
|
||||
void growTables();
|
||||
|
||||
B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
|
||||
{
|
||||
return pair.x == proxyId1 && pair.y == proxyId2;
|
||||
}
|
||||
|
||||
/*
|
||||
// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
|
||||
// This assumes proxyId1 and proxyId2 are 16-bit.
|
||||
B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
|
||||
{
|
||||
int key = (proxyId2 << 16) | proxyId1;
|
||||
key = ~key + (key << 15);
|
||||
key = key ^ (key >> 12);
|
||||
key = key + (key << 2);
|
||||
key = key ^ (key >> 4);
|
||||
key = key * 2057;
|
||||
key = key ^ (key >> 16);
|
||||
return key;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
|
||||
{
|
||||
int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
|
||||
// Thomas Wang's hash
|
||||
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return static_cast<unsigned int>(key);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
|
||||
{
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
|
||||
if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);
|
||||
#endif
|
||||
|
||||
int index = m_hashTable[hash];
|
||||
|
||||
while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
|
||||
{
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if ( index == B3_NULL_PAIR )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
b3Assert(index < m_overlappingPairArray.size());
|
||||
|
||||
return &m_overlappingPairArray[index];
|
||||
}
|
||||
|
||||
virtual bool hasDeferredRemoval()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
|
||||
{
|
||||
m_ghostPairCallback = ghostPairCallback;
|
||||
}
|
||||
*/
|
||||
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
b3AlignedObjectArray<int> m_hashTable;
|
||||
b3AlignedObjectArray<int> m_next;
|
||||
// b3OverlappingPairCallback* m_ghostPairCallback;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
|
||||
///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
|
||||
class b3SortedOverlappingPairCache : public b3OverlappingPairCache
|
||||
{
|
||||
protected:
|
||||
//avoid brute-force finding all the time
|
||||
b3BroadphasePairArray m_overlappingPairArray;
|
||||
|
||||
//during the dispatch, check that user doesn't destroy/create proxy
|
||||
bool m_blockedForChanges;
|
||||
|
||||
///by default, do the removal during the pair traversal
|
||||
bool m_hasDeferredRemoval;
|
||||
|
||||
//if set, use the callback instead of the built in filter in needBroadphaseCollision
|
||||
b3OverlapFilterCallback* m_overlapFilterCallback;
|
||||
|
||||
// b3OverlappingPairCallback* m_ghostPairCallback;
|
||||
|
||||
public:
|
||||
|
||||
b3SortedOverlappingPairCache();
|
||||
virtual ~b3SortedOverlappingPairCache();
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
|
||||
|
||||
void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
|
||||
|
||||
void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
|
||||
|
||||
b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1);
|
||||
|
||||
b3BroadphasePair* findPair(int proxy0,int proxy1);
|
||||
|
||||
|
||||
void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
|
||||
|
||||
virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
|
||||
|
||||
|
||||
inline bool needsBroadphaseCollision(int proxy0,int proxy1) const
|
||||
{
|
||||
if (m_overlapFilterCallback)
|
||||
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
|
||||
|
||||
bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
|
||||
//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
|
||||
|
||||
return collides;
|
||||
}
|
||||
|
||||
b3BroadphasePairArray& getOverlappingPairArray()
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
const b3BroadphasePairArray& getOverlappingPairArray() const
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
b3BroadphasePair* getOverlappingPairArrayPtr()
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
const b3BroadphasePair* getOverlappingPairArrayPtr() const
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
int getNumOverlappingPairs() const
|
||||
{
|
||||
return m_overlappingPairArray.size();
|
||||
}
|
||||
|
||||
b3OverlapFilterCallback* getOverlapFilterCallback()
|
||||
{
|
||||
return m_overlapFilterCallback;
|
||||
}
|
||||
|
||||
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
|
||||
{
|
||||
m_overlapFilterCallback = callback;
|
||||
}
|
||||
|
||||
virtual bool hasDeferredRemoval()
|
||||
{
|
||||
return m_hasDeferredRemoval;
|
||||
}
|
||||
|
||||
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
|
||||
{
|
||||
m_ghostPairCallback = ghostPairCallback;
|
||||
}
|
||||
*/
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
|
||||
class b3NullPairCache : public b3OverlappingPairCache
|
||||
{
|
||||
|
||||
b3BroadphasePairArray m_overlappingPairArray;
|
||||
|
||||
public:
|
||||
|
||||
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
const b3BroadphasePair* getOverlappingPairArrayPtr() const
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
b3BroadphasePairArray& getOverlappingPairArray()
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual int getNumOverlappingPairs() const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual bool hasDeferredRemoval()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
|
||||
// {
|
||||
//
|
||||
// }
|
||||
|
||||
virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
(void) dispatcher;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //B3_OVERLAPPING_PAIR_CACHE_H
|
||||
|
||||
|
59
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h
vendored
Normal file
59
thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h
vendored
Normal file
|
@ -0,0 +1,59 @@
|
|||
|
||||
#ifndef B3_AABB_H
|
||||
#define B3_AABB_H
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Mat3x3.h"
|
||||
|
||||
typedef struct b3Aabb b3Aabb_t;
|
||||
|
||||
struct b3Aabb
|
||||
{
|
||||
union
|
||||
{
|
||||
float m_min[4];
|
||||
b3Float4 m_minVec;
|
||||
int m_minIndices[4];
|
||||
};
|
||||
union
|
||||
{
|
||||
float m_max[4];
|
||||
b3Float4 m_maxVec;
|
||||
int m_signedMaxIndices[4];
|
||||
};
|
||||
};
|
||||
|
||||
inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,
|
||||
b3Float4ConstArg pos,
|
||||
b3QuatConstArg orn,
|
||||
b3Float4* aabbMinOut,b3Float4* aabbMaxOut)
|
||||
{
|
||||
b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);
|
||||
localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);
|
||||
b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);
|
||||
b3Mat3x3 m;
|
||||
m = b3QuatGetRotationMatrix(orn);
|
||||
b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
|
||||
b3Float4 center = b3TransformPoint(localCenter,pos,orn);
|
||||
|
||||
b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),
|
||||
b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),
|
||||
b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),
|
||||
0.f);
|
||||
*aabbMinOut = center-extent;
|
||||
*aabbMaxOut = center+extent;
|
||||
}
|
||||
|
||||
/// conservative test for overlap between two aabbs
|
||||
inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,
|
||||
b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
|
||||
overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;
|
||||
overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;
|
||||
return overlap;
|
||||
}
|
||||
|
||||
#endif //B3_AABB_H
|
93
thirdparty/bullet/src/Bullet3Collision/CMakeLists.txt
vendored
Normal file
93
thirdparty/bullet/src/Bullet3Collision/CMakeLists.txt
vendored
Normal file
|
@ -0,0 +1,93 @@
|
|||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
)
|
||||
|
||||
SET(Bullet3Collision_SRCS
|
||||
BroadPhaseCollision/b3DynamicBvh.cpp
|
||||
BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
|
||||
BroadPhaseCollision/b3OverlappingPairCache.cpp
|
||||
NarrowPhaseCollision/b3ConvexUtility.cpp
|
||||
NarrowPhaseCollision/b3CpuNarrowPhase.cpp
|
||||
)
|
||||
|
||||
SET(Bullet3CollisionBroadPhase_HDRS
|
||||
BroadPhaseCollision/b3BroadphaseCallback.h
|
||||
BroadPhaseCollision/b3DynamicBvh.h
|
||||
BroadPhaseCollision/b3DynamicBvhBroadphase.h
|
||||
BroadPhaseCollision/b3OverlappingPair.h
|
||||
BroadPhaseCollision/b3OverlappingPairCache.h
|
||||
)
|
||||
SET(Bullet3CollisionBroadPhaseShared_HDRS
|
||||
BroadPhaseCollision/shared/b3Aabb.h
|
||||
)
|
||||
|
||||
SET(Bullet3CollisionNarrowPhase_HDRS
|
||||
NarrowPhaseCollision/b3Config.h
|
||||
NarrowPhaseCollision/b3Contact4.h
|
||||
NarrowPhaseCollision/b3ConvexUtility.h
|
||||
NarrowPhaseCollision/b3CpuNarrowPhase.h
|
||||
NarrowPhaseCollision/b3RaycastInfo.h
|
||||
NarrowPhaseCollision/b3RigidBodyCL.h
|
||||
)
|
||||
SET(Bullet3CollisionNarrowPhaseShared_HDRS
|
||||
|
||||
NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
|
||||
NarrowPhaseCollision/shared/b3BvhTraversal.h
|
||||
NarrowPhaseCollision/shared/b3ClipFaces.h
|
||||
NarrowPhaseCollision/shared/b3Collidable.h
|
||||
NarrowPhaseCollision/shared/b3Contact4Data.h
|
||||
NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
|
||||
NarrowPhaseCollision/shared/b3ContactSphereSphere.h
|
||||
NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
|
||||
NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
|
||||
NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
|
||||
NarrowPhaseCollision/shared/b3MprPenetration.h
|
||||
NarrowPhaseCollision/shared/b3NewContactReduction.h
|
||||
NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
|
||||
NarrowPhaseCollision/shared/b3ReduceContacts.h
|
||||
NarrowPhaseCollision/shared/b3RigidBodyData.h
|
||||
NarrowPhaseCollision/shared/b3UpdateAabbs.h
|
||||
)
|
||||
|
||||
SET(Bullet3Collision_HDRS
|
||||
${Bullet3CollisionBroadPhase_HDRS}
|
||||
${Bullet3CollisionBroadPhaseShared_HDRS}
|
||||
${Bullet3CollisionNarrowPhaseShared_HDRS}
|
||||
${Bullet3CollisionNarrowPhase_HDRS}
|
||||
)
|
||||
|
||||
ADD_LIBRARY(Bullet3Collision ${Bullet3Collision_SRCS} ${Bullet3Collision_HDRS})
|
||||
if (BUILD_SHARED_LIBS)
|
||||
target_link_libraries(Bullet3Collision Bullet3Geometry)
|
||||
endif ()
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#FILES_MATCHING requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Collision DESTINATION .)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Collision
|
||||
RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
|
||||
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES FRAMEWORK true)
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES PUBLIC_HEADER "${Bullet3Collision_HDRS}")
|
||||
# Have to list out sub-directories manually:
|
||||
#todo
|
||||
#SET_PROPERTY(SOURCE ${Bullet3CollisionBroadPhase_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadPhaseCollision)
|
||||
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
41
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
vendored
Normal file
41
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
vendored
Normal file
|
@ -0,0 +1,41 @@
|
|||
#ifndef B3_CONFIG_H
|
||||
#define B3_CONFIG_H
|
||||
|
||||
struct b3Config
|
||||
{
|
||||
int m_maxConvexBodies;
|
||||
int m_maxConvexShapes;
|
||||
int m_maxBroadphasePairs;
|
||||
int m_maxContactCapacity;
|
||||
int m_compoundPairCapacity;
|
||||
|
||||
int m_maxVerticesPerFace;
|
||||
int m_maxFacesPerShape;
|
||||
int m_maxConvexVertices;
|
||||
int m_maxConvexIndices;
|
||||
int m_maxConvexUniqueEdges;
|
||||
|
||||
int m_maxCompoundChildShapes;
|
||||
|
||||
int m_maxTriConvexPairCapacity;
|
||||
|
||||
b3Config()
|
||||
:m_maxConvexBodies(128*1024),
|
||||
m_maxVerticesPerFace(64),
|
||||
m_maxFacesPerShape(12),
|
||||
m_maxConvexVertices(8192),
|
||||
m_maxConvexIndices(81920),
|
||||
m_maxConvexUniqueEdges(8192),
|
||||
m_maxCompoundChildShapes(8192),
|
||||
m_maxTriConvexPairCapacity(256*1024)
|
||||
{
|
||||
m_maxConvexShapes = m_maxConvexBodies;
|
||||
m_maxBroadphasePairs = 16*m_maxConvexBodies;
|
||||
m_maxContactCapacity = m_maxBroadphasePairs;
|
||||
m_compoundPairCapacity = 1024*1024;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif//B3_CONFIG_H
|
||||
|
46
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h
vendored
Normal file
46
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h
vendored
Normal file
|
@ -0,0 +1,46 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_CONTACT4_H
|
||||
#define B3_CONTACT4_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
int getBodyA()const {return abs(m_bodyAPtrAndSignBit);}
|
||||
int getBodyB()const {return abs(m_bodyBPtrAndSignBit);}
|
||||
bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;}
|
||||
bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;}
|
||||
// todo. make it safer
|
||||
int& getBatchIdx() { return m_batchIdx; }
|
||||
const int& getBatchIdx() const { return m_batchIdx; }
|
||||
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); }
|
||||
void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); }
|
||||
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); }
|
||||
void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
|
||||
|
||||
//float& getNPoints() { return m_worldNormal[3]; }
|
||||
int getNPoints() const { return (int) m_worldNormalOnB.w; }
|
||||
|
||||
float getPenetration(int idx) const { return m_worldPosB[idx].w; }
|
||||
|
||||
bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
|
||||
};
|
||||
|
||||
#endif //B3_CONTACT4_H
|
520
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
vendored
Normal file
520
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
vendored
Normal file
|
@ -0,0 +1,520 @@
|
|||
/*
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
|
||||
#include "b3ConvexUtility.h"
|
||||
#include "Bullet3Geometry/b3ConvexHullComputer.h"
|
||||
#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3HashMap.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
b3ConvexUtility::~b3ConvexUtility()
|
||||
{
|
||||
}
|
||||
|
||||
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
|
||||
{
|
||||
|
||||
|
||||
|
||||
b3ConvexHullComputer conv;
|
||||
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> faceNormals;
|
||||
int numFaces = conv.faces.size();
|
||||
faceNormals.resize(numFaces);
|
||||
b3ConvexHullComputer* convexUtil = &conv;
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3MyFace> tmpFaces;
|
||||
tmpFaces.resize(numFaces);
|
||||
|
||||
int numVertices = convexUtil->vertices.size();
|
||||
m_vertices.resize(numVertices);
|
||||
for (int p=0;p<numVertices;p++)
|
||||
{
|
||||
m_vertices[p] = convexUtil->vertices[p];
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<numFaces;i++)
|
||||
{
|
||||
int face = convexUtil->faces[i];
|
||||
//printf("face=%d\n",face);
|
||||
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
|
||||
const b3ConvexHullComputer::Edge* edge = firstEdge;
|
||||
|
||||
b3Vector3 edges[3];
|
||||
int numEdges = 0;
|
||||
//compute face normals
|
||||
|
||||
do
|
||||
{
|
||||
|
||||
int src = edge->getSourceVertex();
|
||||
tmpFaces[i].m_indices.push_back(src);
|
||||
int targ = edge->getTargetVertex();
|
||||
b3Vector3 wa = convexUtil->vertices[src];
|
||||
|
||||
b3Vector3 wb = convexUtil->vertices[targ];
|
||||
b3Vector3 newEdge = wb-wa;
|
||||
newEdge.normalize();
|
||||
if (numEdges<2)
|
||||
edges[numEdges++] = newEdge;
|
||||
|
||||
edge = edge->getNextEdgeOfFace();
|
||||
} while (edge!=firstEdge);
|
||||
|
||||
b3Scalar planeEq = 1e30f;
|
||||
|
||||
|
||||
if (numEdges==2)
|
||||
{
|
||||
faceNormals[i] = edges[0].cross(edges[1]);
|
||||
faceNormals[i].normalize();
|
||||
tmpFaces[i].m_plane[0] = faceNormals[i].getX();
|
||||
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
|
||||
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
|
||||
tmpFaces[i].m_plane[3] = planeEq;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Assert(0);//degenerate?
|
||||
faceNormals[i].setZero();
|
||||
}
|
||||
|
||||
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
|
||||
{
|
||||
b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
|
||||
if (planeEq>eq)
|
||||
{
|
||||
planeEq=eq;
|
||||
}
|
||||
}
|
||||
tmpFaces[i].m_plane[3] = -planeEq;
|
||||
}
|
||||
|
||||
//merge coplanar faces and copy them to m_polyhedron
|
||||
|
||||
b3Scalar faceWeldThreshold= 0.999f;
|
||||
b3AlignedObjectArray<int> todoFaces;
|
||||
for (int i=0;i<tmpFaces.size();i++)
|
||||
todoFaces.push_back(i);
|
||||
|
||||
while (todoFaces.size())
|
||||
{
|
||||
b3AlignedObjectArray<int> coplanarFaceGroup;
|
||||
int refFace = todoFaces[todoFaces.size()-1];
|
||||
|
||||
coplanarFaceGroup.push_back(refFace);
|
||||
b3MyFace& faceA = tmpFaces[refFace];
|
||||
todoFaces.pop_back();
|
||||
|
||||
b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
|
||||
for (int j=todoFaces.size()-1;j>=0;j--)
|
||||
{
|
||||
int i = todoFaces[j];
|
||||
b3MyFace& faceB = tmpFaces[i];
|
||||
b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
|
||||
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
|
||||
{
|
||||
coplanarFaceGroup.push_back(i);
|
||||
todoFaces.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool did_merge = false;
|
||||
if (coplanarFaceGroup.size()>1)
|
||||
{
|
||||
//do the merge: use Graham Scan 2d convex hull
|
||||
|
||||
b3AlignedObjectArray<b3GrahamVector3> orgpoints;
|
||||
b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0);
|
||||
|
||||
for (int i=0;i<coplanarFaceGroup.size();i++)
|
||||
{
|
||||
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
|
||||
|
||||
b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
|
||||
b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
|
||||
averageFaceNormal+=faceNormal;
|
||||
for (int f=0;f<face.m_indices.size();f++)
|
||||
{
|
||||
int orgIndex = face.m_indices[f];
|
||||
b3Vector3 pt = m_vertices[orgIndex];
|
||||
|
||||
bool found = false;
|
||||
|
||||
for (int i=0;i<orgpoints.size();i++)
|
||||
{
|
||||
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
|
||||
if (orgpoints[i].m_orgIndex == orgIndex)
|
||||
{
|
||||
found=true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found)
|
||||
orgpoints.push_back(b3GrahamVector3(pt,orgIndex));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3MyFace combinedFace;
|
||||
for (int i=0;i<4;i++)
|
||||
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
|
||||
|
||||
b3AlignedObjectArray<b3GrahamVector3> hull;
|
||||
|
||||
averageFaceNormal.normalize();
|
||||
b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
|
||||
|
||||
for (int i=0;i<hull.size();i++)
|
||||
{
|
||||
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
|
||||
for(int k = 0; k < orgpoints.size(); k++)
|
||||
{
|
||||
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
|
||||
{
|
||||
orgpoints[k].m_orgIndex = -1; // invalidate...
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// are there rejected vertices?
|
||||
bool reject_merge = false;
|
||||
|
||||
|
||||
|
||||
for(int i = 0; i < orgpoints.size(); i++) {
|
||||
if(orgpoints[i].m_orgIndex == -1)
|
||||
continue; // this is in the hull...
|
||||
// this vertex is rejected -- is anybody else using this vertex?
|
||||
for(int j = 0; j < tmpFaces.size(); j++) {
|
||||
|
||||
b3MyFace& face = tmpFaces[j];
|
||||
// is this a face of the current coplanar group?
|
||||
bool is_in_current_group = false;
|
||||
for(int k = 0; k < coplanarFaceGroup.size(); k++) {
|
||||
if(coplanarFaceGroup[k] == j) {
|
||||
is_in_current_group = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(is_in_current_group) // ignore this face...
|
||||
continue;
|
||||
// does this face use this rejected vertex?
|
||||
for(int v = 0; v < face.m_indices.size(); v++) {
|
||||
if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
|
||||
// this rejected vertex is used in another face -- reject merge
|
||||
reject_merge = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(reject_merge)
|
||||
break;
|
||||
}
|
||||
if(reject_merge)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!reject_merge)
|
||||
{
|
||||
// do this merge!
|
||||
did_merge = true;
|
||||
m_faces.push_back(combinedFace);
|
||||
}
|
||||
}
|
||||
if(!did_merge)
|
||||
{
|
||||
for (int i=0;i<coplanarFaceGroup.size();i++)
|
||||
{
|
||||
b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
|
||||
m_faces.push_back(face);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
initialize();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline bool IsAlmostZero(const b3Vector3& v)
|
||||
{
|
||||
if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
struct b3InternalVertexPair
|
||||
{
|
||||
b3InternalVertexPair(short int v0,short int v1)
|
||||
:m_v0(v0),
|
||||
m_v1(v1)
|
||||
{
|
||||
if (m_v1>m_v0)
|
||||
b3Swap(m_v0,m_v1);
|
||||
}
|
||||
short int m_v0;
|
||||
short int m_v1;
|
||||
int getHash() const
|
||||
{
|
||||
return m_v0+(m_v1<<16);
|
||||
}
|
||||
bool equals(const b3InternalVertexPair& other) const
|
||||
{
|
||||
return m_v0==other.m_v0 && m_v1==other.m_v1;
|
||||
}
|
||||
};
|
||||
|
||||
struct b3InternalEdge
|
||||
{
|
||||
b3InternalEdge()
|
||||
:m_face0(-1),
|
||||
m_face1(-1)
|
||||
{
|
||||
}
|
||||
short int m_face0;
|
||||
short int m_face1;
|
||||
};
|
||||
|
||||
//
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
bool b3ConvexUtility::testContainment() const
|
||||
{
|
||||
for(int p=0;p<8;p++)
|
||||
{
|
||||
b3Vector3 LocalPt;
|
||||
if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
|
||||
else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
|
||||
else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
|
||||
else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
|
||||
else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
|
||||
else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
|
||||
else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
|
||||
else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
|
||||
|
||||
for(int i=0;i<m_faces.size();i++)
|
||||
{
|
||||
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
|
||||
const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
|
||||
if(d>0.0f)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
void b3ConvexUtility::initialize()
|
||||
{
|
||||
|
||||
b3HashMap<b3InternalVertexPair,b3InternalEdge> edges;
|
||||
|
||||
b3Scalar TotalArea = 0.0f;
|
||||
|
||||
m_localCenter.setValue(0, 0, 0);
|
||||
for(int i=0;i<m_faces.size();i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
int NbTris = numVertices;
|
||||
for(int j=0;j<NbTris;j++)
|
||||
{
|
||||
int k = (j+1)%numVertices;
|
||||
b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
|
||||
b3InternalEdge* edptr = edges.find(vp);
|
||||
b3Vector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
|
||||
edge.normalize();
|
||||
|
||||
bool found = false;
|
||||
b3Vector3 diff,diff2;
|
||||
|
||||
for (int p=0;p<m_uniqueEdges.size();p++)
|
||||
{
|
||||
diff = m_uniqueEdges[p]-edge;
|
||||
diff2 = m_uniqueEdges[p]+edge;
|
||||
|
||||
// if ((diff.length2()==0.f) ||
|
||||
// (diff2.length2()==0.f))
|
||||
|
||||
if (IsAlmostZero(diff) ||
|
||||
IsAlmostZero(diff2))
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found)
|
||||
{
|
||||
m_uniqueEdges.push_back(edge);
|
||||
}
|
||||
|
||||
if (edptr)
|
||||
{
|
||||
//TBD: figure out why I added this assert
|
||||
// b3Assert(edptr->m_face0>=0);
|
||||
// b3Assert(edptr->m_face1<0);
|
||||
edptr->m_face1 = i;
|
||||
} else
|
||||
{
|
||||
b3InternalEdge ed;
|
||||
ed.m_face0 = i;
|
||||
edges.insert(vp,ed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_CONNECTED_FACES
|
||||
for(int i=0;i<m_faces.size();i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
m_faces[i].m_connectedFaces.resize(numVertices);
|
||||
|
||||
for(int j=0;j<numVertices;j++)
|
||||
{
|
||||
int k = (j+1)%numVertices;
|
||||
b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
|
||||
b3InternalEdge* edptr = edges.find(vp);
|
||||
b3Assert(edptr);
|
||||
b3Assert(edptr->m_face0>=0);
|
||||
b3Assert(edptr->m_face1>=0);
|
||||
|
||||
int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
|
||||
m_faces[i].m_connectedFaces[j] = connectedFace;
|
||||
}
|
||||
}
|
||||
#endif//USE_CONNECTED_FACES
|
||||
|
||||
for(int i=0;i<m_faces.size();i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
int NbTris = numVertices-2;
|
||||
|
||||
const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
|
||||
for(int j=1;j<=NbTris;j++)
|
||||
{
|
||||
int k = (j+1)%numVertices;
|
||||
const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
|
||||
const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
|
||||
b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
|
||||
b3Vector3 Center = (p0+p1+p2)/3.0f;
|
||||
m_localCenter += Area * Center;
|
||||
TotalArea += Area;
|
||||
}
|
||||
}
|
||||
m_localCenter /= TotalArea;
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
if(1)
|
||||
{
|
||||
m_radius = FLT_MAX;
|
||||
for(int i=0;i<m_faces.size();i++)
|
||||
{
|
||||
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
|
||||
const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
|
||||
if(dist<m_radius)
|
||||
m_radius = dist;
|
||||
}
|
||||
|
||||
|
||||
b3Scalar MinX = FLT_MAX;
|
||||
b3Scalar MinY = FLT_MAX;
|
||||
b3Scalar MinZ = FLT_MAX;
|
||||
b3Scalar MaxX = -FLT_MAX;
|
||||
b3Scalar MaxY = -FLT_MAX;
|
||||
b3Scalar MaxZ = -FLT_MAX;
|
||||
for(int i=0; i<m_vertices.size(); i++)
|
||||
{
|
||||
const b3Vector3& pt = m_vertices[i];
|
||||
if(pt.getX()<MinX) MinX = pt.getX();
|
||||
if(pt.getX()>MaxX) MaxX = pt.getX();
|
||||
if(pt.getY()<MinY) MinY = pt.getY();
|
||||
if(pt.getY()>MaxY) MaxY = pt.getY();
|
||||
if(pt.getZ()<MinZ) MinZ = pt.getZ();
|
||||
if(pt.getZ()>MaxZ) MaxZ = pt.getZ();
|
||||
}
|
||||
mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
|
||||
mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
|
||||
|
||||
|
||||
|
||||
// const b3Scalar r = m_radius / sqrtf(2.0f);
|
||||
const b3Scalar r = m_radius / sqrtf(3.0f);
|
||||
const int LargestExtent = mE.maxAxis();
|
||||
const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
|
||||
m_extents[0] = m_extents[1] = m_extents[2] = r;
|
||||
m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
|
||||
bool FoundBox = false;
|
||||
for(int j=0;j<1024;j++)
|
||||
{
|
||||
if(testContainment())
|
||||
{
|
||||
FoundBox = true;
|
||||
break;
|
||||
}
|
||||
|
||||
m_extents[LargestExtent] -= Step;
|
||||
}
|
||||
if(!FoundBox)
|
||||
{
|
||||
m_extents[0] = m_extents[1] = m_extents[2] = r;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Refine the box
|
||||
const b3Scalar Step = (m_radius - r)/1024.0f;
|
||||
const int e0 = (1<<LargestExtent) & 3;
|
||||
const int e1 = (1<<e0) & 3;
|
||||
|
||||
for(int j=0;j<1024;j++)
|
||||
{
|
||||
const b3Scalar Saved0 = m_extents[e0];
|
||||
const b3Scalar Saved1 = m_extents[e1];
|
||||
m_extents[e0] += Step;
|
||||
m_extents[e1] += Step;
|
||||
|
||||
if(!testContainment())
|
||||
{
|
||||
m_extents[e0] = Saved0;
|
||||
m_extents[e1] = Saved1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
62
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
vendored
Normal file
62
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
vendored
Normal file
|
@ -0,0 +1,62 @@
|
|||
|
||||
/*
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#ifndef _BT_CONVEX_UTILITY_H
|
||||
#define _BT_CONVEX_UTILITY_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
|
||||
|
||||
|
||||
|
||||
struct b3MyFace
|
||||
{
|
||||
b3AlignedObjectArray<int> m_indices;
|
||||
b3Scalar m_plane[4];
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
|
||||
{
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3Vector3 m_localCenter;
|
||||
b3Vector3 m_extents;
|
||||
b3Vector3 mC;
|
||||
b3Vector3 mE;
|
||||
b3Scalar m_radius;
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> m_vertices;
|
||||
b3AlignedObjectArray<b3MyFace> m_faces;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
|
||||
|
||||
b3ConvexUtility()
|
||||
{
|
||||
}
|
||||
virtual ~b3ConvexUtility();
|
||||
|
||||
bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true);
|
||||
|
||||
void initialize();
|
||||
bool testContainment() const;
|
||||
|
||||
|
||||
|
||||
};
|
||||
#endif
|
||||
|
323
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
vendored
Normal file
323
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
vendored
Normal file
|
@ -0,0 +1,323 @@
|
|||
#include "b3CpuNarrowPhase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
|
||||
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
|
||||
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
|
||||
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
|
||||
b3Config m_config;
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
b3AlignedObjectArray<b3Vector3> m_convexVertices;
|
||||
b3AlignedObjectArray<int> m_convexIndices;
|
||||
b3AlignedObjectArray<b3GpuFace> m_convexFaces;
|
||||
|
||||
b3AlignedObjectArray<b3Contact4Data> m_contacts;
|
||||
|
||||
int m_numAcceleratedShapes;
|
||||
};
|
||||
|
||||
|
||||
const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
|
||||
{
|
||||
return m_data->m_contacts;
|
||||
}
|
||||
|
||||
b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
|
||||
{
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
|
||||
{
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
|
||||
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
|
||||
{
|
||||
m_data = new b3CpuNarrowPhaseInternalData;
|
||||
m_data->m_config = config;
|
||||
m_data->m_numAcceleratedShapes = 0;
|
||||
}
|
||||
|
||||
b3CpuNarrowPhase::~b3CpuNarrowPhase()
|
||||
{
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
|
||||
{
|
||||
int nPairs = pairs.size();
|
||||
int numContacts = 0;
|
||||
int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
|
||||
m_data->m_contacts.resize(maxContactCapacity);
|
||||
|
||||
for (int i=0;i<nPairs;i++)
|
||||
{
|
||||
int bodyIndexA = pairs[i].x;
|
||||
int bodyIndexB = pairs[i].y;
|
||||
int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
|
||||
{
|
||||
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
//printf("convex-sphere\n");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
|
||||
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
//printf("pairs[i].z=%d\n",pairs[i].z);
|
||||
//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
|
||||
// m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
||||
int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
|
||||
m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity);
|
||||
|
||||
|
||||
if (contactIndex>=0)
|
||||
{
|
||||
pairs[i].z = contactIndex;
|
||||
}
|
||||
// printf("plane-convex\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
m_data->m_contacts.resize(numContacts);
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
{
|
||||
int collidableIndex = allocateCollidable();
|
||||
if (collidableIndex<0)
|
||||
return collidableIndex;
|
||||
|
||||
|
||||
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
|
||||
col.m_shapeType = SHAPE_CONVEX_HULL;
|
||||
col.m_shapeIndex = -1;
|
||||
|
||||
|
||||
{
|
||||
b3Vector3 localCenter=b3MakeVector3(0,0,0);
|
||||
for (int i=0;i<utilPtr->m_vertices.size();i++)
|
||||
localCenter+=utilPtr->m_vertices[i];
|
||||
localCenter*= (1.f/utilPtr->m_vertices.size());
|
||||
utilPtr->m_localCenter = localCenter;
|
||||
|
||||
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
|
||||
}
|
||||
|
||||
if (col.m_shapeIndex>=0)
|
||||
{
|
||||
b3Aabb aabb;
|
||||
|
||||
b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f);
|
||||
b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f);
|
||||
|
||||
for (int i=0;i<utilPtr->m_vertices.size();i++)
|
||||
{
|
||||
myAabbMin.setMin(utilPtr->m_vertices[i]);
|
||||
myAabbMax.setMax(utilPtr->m_vertices[i]);
|
||||
}
|
||||
aabb.m_min[0] = myAabbMin[0];
|
||||
aabb.m_min[1] = myAabbMin[1];
|
||||
aabb.m_min[2] = myAabbMin[2];
|
||||
aabb.m_minIndices[3] = 0;
|
||||
|
||||
aabb.m_max[0] = myAabbMax[0];
|
||||
aabb.m_max[1] = myAabbMax[1];
|
||||
aabb.m_max[2] = myAabbMax[2];
|
||||
aabb.m_signedMaxIndices[3] = 0;
|
||||
|
||||
m_data->m_localShapeAABBCPU.push_back(aabb);
|
||||
|
||||
}
|
||||
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::allocateCollidable()
|
||||
{
|
||||
int curSize = m_data->m_collidablesCPU.size();
|
||||
if (curSize<m_data->m_config.m_maxConvexShapes)
|
||||
{
|
||||
m_data->m_collidablesCPU.expand();
|
||||
return curSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes);
|
||||
}
|
||||
return -1;
|
||||
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
|
||||
{
|
||||
b3AlignedObjectArray<b3Vector3> verts;
|
||||
|
||||
unsigned char* vts = (unsigned char*) vertices;
|
||||
for (int i=0;i<numVertices;i++)
|
||||
{
|
||||
float* vertex = (float*) &vts[i*strideInBytes];
|
||||
verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
|
||||
}
|
||||
|
||||
b3ConvexUtility* utilPtr = new b3ConvexUtility();
|
||||
bool merge = true;
|
||||
if (numVertices)
|
||||
{
|
||||
utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
|
||||
}
|
||||
|
||||
int collidableIndex = registerConvexHullShape(utilPtr);
|
||||
|
||||
delete utilPtr;
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
|
||||
{
|
||||
|
||||
m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1);
|
||||
m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
|
||||
|
||||
|
||||
b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
|
||||
convex.mC = convexPtr->mC;
|
||||
convex.mE = convexPtr->mE;
|
||||
convex.m_extents= convexPtr->m_extents;
|
||||
convex.m_localCenter = convexPtr->m_localCenter;
|
||||
convex.m_radius = convexPtr->m_radius;
|
||||
|
||||
convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
|
||||
int edgeOffset = m_data->m_uniqueEdges.size();
|
||||
convex.m_uniqueEdgesOffset = edgeOffset;
|
||||
|
||||
m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
|
||||
|
||||
//convex data here
|
||||
int i;
|
||||
for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
|
||||
{
|
||||
m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
|
||||
}
|
||||
|
||||
int faceOffset = m_data->m_convexFaces.size();
|
||||
convex.m_faceOffset = faceOffset;
|
||||
convex.m_numFaces = convexPtr->m_faces.size();
|
||||
|
||||
m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
|
||||
|
||||
|
||||
for (i=0;i<convexPtr->m_faces.size();i++)
|
||||
{
|
||||
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
|
||||
convexPtr->m_faces[i].m_plane[1],
|
||||
convexPtr->m_faces[i].m_plane[2],
|
||||
convexPtr->m_faces[i].m_plane[3]);
|
||||
|
||||
|
||||
int indexOffset = m_data->m_convexIndices.size();
|
||||
int numIndices = convexPtr->m_faces[i].m_indices.size();
|
||||
m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
|
||||
m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
|
||||
m_data->m_convexIndices.resize(indexOffset+numIndices);
|
||||
for (int p=0;p<numIndices;p++)
|
||||
{
|
||||
m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
|
||||
}
|
||||
}
|
||||
|
||||
convex.m_numVertices = convexPtr->m_vertices.size();
|
||||
int vertexOffset = m_data->m_convexVertices.size();
|
||||
convex.m_vertexOffset =vertexOffset;
|
||||
|
||||
m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
|
||||
for (int i=0;i<convexPtr->m_vertices.size();i++)
|
||||
{
|
||||
m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
|
||||
}
|
||||
|
||||
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
|
||||
|
||||
|
||||
|
||||
return m_data->m_numAcceleratedShapes++;
|
||||
}
|
||||
|
||||
const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
|
||||
{
|
||||
return m_data->m_localShapeAABBCPU[collidableIndex];
|
||||
}
|
105
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
vendored
Normal file
105
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
vendored
Normal file
|
@ -0,0 +1,105 @@
|
|||
#ifndef B3_CPU_NARROWPHASE_H
|
||||
#define B3_CPU_NARROWPHASE_H
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
class b3CpuNarrowPhase
|
||||
{
|
||||
protected:
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData* m_data;
|
||||
int m_acceleratedCompanionShapeIndex;
|
||||
int m_planeBodyIndex;
|
||||
int m_static0Index;
|
||||
|
||||
int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col);
|
||||
int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
|
||||
|
||||
public:
|
||||
|
||||
|
||||
|
||||
|
||||
b3CpuNarrowPhase(const struct b3Config& config);
|
||||
|
||||
virtual ~b3CpuNarrowPhase(void);
|
||||
|
||||
int registerSphereShape(float radius);
|
||||
int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
|
||||
|
||||
int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
|
||||
int registerFace(const b3Vector3& faceNormal, float faceConstant);
|
||||
|
||||
int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling);
|
||||
|
||||
//do they need to be merged?
|
||||
|
||||
int registerConvexHullShape(b3ConvexUtility* utilPtr);
|
||||
int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
|
||||
|
||||
//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
|
||||
void setObjectTransform(const float* position, const float* orientation , int bodyIndex);
|
||||
|
||||
void writeAllBodiesToGpu();
|
||||
void reset();
|
||||
void readbackAllBodiesToCpu();
|
||||
bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const;
|
||||
|
||||
void setObjectTransformCpu(float* position, float* orientation , int bodyIndex);
|
||||
void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
|
||||
|
||||
|
||||
//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
|
||||
virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
|
||||
|
||||
|
||||
|
||||
const struct b3RigidBodyData* getBodiesCpu() const;
|
||||
//struct b3RigidBodyData* getBodiesCpu();
|
||||
|
||||
int getNumBodiesGpu() const;
|
||||
|
||||
|
||||
int getNumBodyInertiasGpu() const;
|
||||
|
||||
|
||||
const struct b3Collidable* getCollidablesCpu() const;
|
||||
int getNumCollidablesGpu() const;
|
||||
|
||||
|
||||
/*const struct b3Contact4* getContactsCPU() const;
|
||||
|
||||
|
||||
int getNumContactsGpu() const;
|
||||
*/
|
||||
|
||||
const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
|
||||
|
||||
|
||||
int getNumRigidBodies() const;
|
||||
|
||||
int allocateCollidable();
|
||||
|
||||
int getStatic0Index() const
|
||||
{
|
||||
return m_static0Index;
|
||||
}
|
||||
b3Collidable& getCollidableCpu(int collidableIndex);
|
||||
const b3Collidable& getCollidableCpu(int collidableIndex) const;
|
||||
|
||||
const b3CpuNarrowPhaseInternalData* getInternalData() const
|
||||
{
|
||||
return m_data;
|
||||
}
|
||||
|
||||
const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
|
||||
};
|
||||
|
||||
#endif //B3_CPU_NARROWPHASE_H
|
||||
|
24
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h
vendored
Normal file
24
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h
vendored
Normal file
|
@ -0,0 +1,24 @@
|
|||
|
||||
#ifndef B3_RAYCAST_INFO_H
|
||||
#define B3_RAYCAST_INFO_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
|
||||
{
|
||||
b3Vector3 m_from;
|
||||
b3Vector3 m_to;
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
|
||||
{
|
||||
b3Scalar m_hitFraction;
|
||||
int m_hitBody;
|
||||
int m_hitResult1;
|
||||
int m_hitResult2;
|
||||
b3Vector3 m_hitPoint;
|
||||
b3Vector3 m_hitNormal;
|
||||
};
|
||||
|
||||
#endif //B3_RAYCAST_INFO_H
|
||||
|
30
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h
vendored
Normal file
30
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h
vendored
Normal file
|
@ -0,0 +1,30 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_RIGID_BODY_CL
|
||||
#define B3_RIGID_BODY_CL
|
||||
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
|
||||
inline float b3GetInvMass(const b3RigidBodyData& body)
|
||||
{
|
||||
return body.m_invMass;
|
||||
}
|
||||
|
||||
|
||||
#endif//B3_RIGID_BODY_CL
|
20
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
vendored
Normal file
20
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
vendored
Normal file
|
@ -0,0 +1,20 @@
|
|||
|
||||
#ifndef B3_BVH_SUBTREE_INFO_DATA_H
|
||||
#define B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
|
||||
|
||||
struct b3BvhSubtreeInfoData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes, points to the root of the subtree
|
||||
int m_rootNodeIndex;
|
||||
//4 bytes
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
};
|
||||
|
||||
#endif //B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
126
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
vendored
Normal file
126
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
vendored
Normal file
|
@ -0,0 +1,126 @@
|
|||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
|
||||
|
||||
|
||||
// work-in-progress
|
||||
void b3BvhTraversal( __global const b3Int4* pairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Int4* concavePairsOut,
|
||||
__global volatile int* numConcavePairsOut,
|
||||
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
|
||||
__global const b3QuantizedBvhNode* quantizedNodesRoot,
|
||||
__global const b3BvhInfo* bvhInfos,
|
||||
int numPairs,
|
||||
int maxNumConcavePairsCapacity,
|
||||
int id)
|
||||
{
|
||||
|
||||
int bodyIndexA = pairs[id].x;
|
||||
int bodyIndexB = pairs[id].y;
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
//once the broadphase avoids static-static pairs, we can remove this test
|
||||
if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
|
||||
return;
|
||||
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
|
||||
if (shapeTypeB!=SHAPE_CONVEX_HULL &&
|
||||
shapeTypeB!=SHAPE_SPHERE &&
|
||||
shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
)
|
||||
return;
|
||||
|
||||
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
|
||||
|
||||
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
|
||||
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
|
||||
b3Float4 bvhQuantization = bvhInfo.m_quantization;
|
||||
int numSubtreeHeaders = bvhInfo.m_numSubTrees;
|
||||
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
|
||||
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
|
||||
|
||||
|
||||
unsigned short int quantizedQueryAabbMin[3];
|
||||
unsigned short int quantizedQueryAabbMax[3];
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
|
||||
for (int i=0;i<numSubtreeHeaders;i++)
|
||||
{
|
||||
b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
|
||||
|
||||
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
|
||||
if (overlap != 0)
|
||||
{
|
||||
int startNodeIndex = subtree.m_rootNodeIndex;
|
||||
int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
|
||||
int curIndex = startNodeIndex;
|
||||
int escapeIndex;
|
||||
int isLeafNode;
|
||||
int aabbOverlap;
|
||||
while (curIndex < endNodeIndex)
|
||||
{
|
||||
b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
|
||||
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
|
||||
isLeafNode = b3IsLeaf(&rootNode);
|
||||
if (aabbOverlap)
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
int triangleIndex = b3GetTriangleIndex(&rootNode);
|
||||
if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
||||
int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
|
||||
for (int b=0;b<numChildrenB;b++)
|
||||
{
|
||||
if ((pairIdx+b)<maxNumConcavePairsCapacity)
|
||||
{
|
||||
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
|
||||
concavePairsOut[pairIdx+b] = newPair;
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
int pairIdx = b3AtomicInc(numConcavePairsOut);
|
||||
if (pairIdx<maxNumConcavePairsCapacity)
|
||||
{
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
|
||||
concavePairsOut[pairIdx] = newPair;
|
||||
}
|
||||
}
|
||||
}
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
escapeIndex = b3GetEscapeIndex(&rootNode);
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
188
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
vendored
Normal file
188
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h
vendored
Normal file
|
@ -0,0 +1,188 @@
|
|||
#ifndef B3_CLIP_FACES_H
|
||||
#define B3_CLIP_FACES_H
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
|
||||
inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t)
|
||||
{
|
||||
return b3MakeFloat4( a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
}
|
||||
|
||||
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
||||
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut)
|
||||
{
|
||||
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
//double-check next test
|
||||
// if (numVertsIn < 2)
|
||||
// return 0;
|
||||
|
||||
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex=pVtxIn[ve];
|
||||
de = b3Dot(planeNormalWS,endVertex)+planeEqWS;
|
||||
if (ds<0)
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
firstVertex = endVertex;
|
||||
ds = de;
|
||||
}
|
||||
return numVertsOut;
|
||||
}
|
||||
|
||||
|
||||
__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
__global b3Float4* worldVertsB2,
|
||||
int vertexFaceCapacity,
|
||||
int pairIndex
|
||||
)
|
||||
{
|
||||
// int i = get_global_id(0);
|
||||
//int pairIndex = i;
|
||||
int i = pairIndex;
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
// if (i<numPairs)
|
||||
{
|
||||
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
|
||||
// int bodyIndexA = pairs[i].x;
|
||||
// int bodyIndexB = pairs[i].y;
|
||||
|
||||
int numLocalContactsOut = 0;
|
||||
|
||||
int capacityWorldVertsB2 = vertexFaceCapacity;
|
||||
|
||||
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];
|
||||
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];
|
||||
|
||||
|
||||
{
|
||||
__global b3Int4* clippingFaces = clippingFacesOut;
|
||||
|
||||
|
||||
int closestFaceA = clippingFaces[pairIndex].x;
|
||||
// int closestFaceB = clippingFaces[pairIndex].y;
|
||||
int numVertsInA = clippingFaces[pairIndex].z;
|
||||
int numVertsInB = clippingFaces[pairIndex].w;
|
||||
|
||||
int numVertsOut = 0;
|
||||
|
||||
if (closestFaceA>=0)
|
||||
{
|
||||
|
||||
|
||||
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
|
||||
for(int e0=0;e0<numVertsInA;e0++)
|
||||
{
|
||||
const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0];
|
||||
const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)];
|
||||
const b3Float4 WorldEdge0 = aw - bw;
|
||||
b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
|
||||
b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = aw;
|
||||
float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1);
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS=planeEqWS1;
|
||||
numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut);
|
||||
__global b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsInB = numVertsOut;
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
|
||||
float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]);
|
||||
|
||||
for (int i=0;i<numVertsInB;i++)
|
||||
{
|
||||
float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS;
|
||||
if (depth <=minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
/*
|
||||
static float maxDepth = 0.f;
|
||||
if (depth < maxDepth)
|
||||
{
|
||||
maxDepth = depth;
|
||||
if (maxDepth < -10)
|
||||
{
|
||||
printf("error at framecount %d?\n",myframecount);
|
||||
}
|
||||
printf("maxDepth = %f\n", maxDepth);
|
||||
|
||||
}
|
||||
*/
|
||||
if (depth <=maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
clippingFaces[pairIndex].w =numLocalContactsOut;
|
||||
|
||||
|
||||
}
|
||||
|
||||
for (int i=0;i<numLocalContactsOut;i++)
|
||||
pVtxIn[i] = pVtxOut[i];
|
||||
|
||||
}// if (hasSeparatingAxis[i])
|
||||
}// if (i<numPairs)
|
||||
|
||||
}
|
||||
|
||||
#endif //B3_CLIP_FACES_H
|
||||
|
76
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h
vendored
Normal file
76
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h
vendored
Normal file
|
@ -0,0 +1,76 @@
|
|||
|
||||
#ifndef B3_COLLIDABLE_H
|
||||
#define B3_COLLIDABLE_H
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
enum b3ShapeTypes
|
||||
{
|
||||
SHAPE_HEIGHT_FIELD=1,
|
||||
|
||||
SHAPE_CONVEX_HULL=3,
|
||||
SHAPE_PLANE=4,
|
||||
SHAPE_CONCAVE_TRIMESH=5,
|
||||
SHAPE_COMPOUND_OF_CONVEX_HULLS=6,
|
||||
SHAPE_SPHERE=7,
|
||||
MAX_NUM_SHAPE_TYPES,
|
||||
};
|
||||
|
||||
typedef struct b3Collidable b3Collidable_t;
|
||||
|
||||
|
||||
struct b3Collidable
|
||||
{
|
||||
union {
|
||||
int m_numChildShapes;
|
||||
int m_bvhIndex;
|
||||
};
|
||||
union
|
||||
{
|
||||
float m_radius;
|
||||
int m_compoundBvhIndex;
|
||||
};
|
||||
|
||||
int m_shapeType;
|
||||
union
|
||||
{
|
||||
int m_shapeIndex;
|
||||
float m_height;
|
||||
};
|
||||
};
|
||||
|
||||
typedef struct b3GpuChildShape b3GpuChildShape_t;
|
||||
struct b3GpuChildShape
|
||||
{
|
||||
b3Float4 m_childPosition;
|
||||
b3Quat m_childOrientation;
|
||||
union
|
||||
{
|
||||
int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
int m_capsuleAxis;
|
||||
};
|
||||
union
|
||||
{
|
||||
float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_numChildShapes;//used for compound shape
|
||||
};
|
||||
union
|
||||
{
|
||||
float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_collidableShapeIndex;
|
||||
};
|
||||
int m_shapeType;
|
||||
};
|
||||
|
||||
struct b3CompoundOverlappingPair
|
||||
{
|
||||
int m_bodyIndexA;
|
||||
int m_bodyIndexB;
|
||||
// int m_pairType;
|
||||
int m_childShapeIndexA;
|
||||
int m_childShapeIndexB;
|
||||
};
|
||||
|
||||
#endif //B3_COLLIDABLE_H
|
40
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h
vendored
Normal file
40
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h
vendored
Normal file
|
@ -0,0 +1,40 @@
|
|||
#ifndef B3_CONTACT4DATA_H
|
||||
#define B3_CONTACT4DATA_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
typedef struct b3Contact4Data b3Contact4Data_t;
|
||||
|
||||
struct b3Contact4Data
|
||||
{
|
||||
b3Float4 m_worldPosB[4];
|
||||
// b3Float4 m_localPosA[4];
|
||||
// b3Float4 m_localPosB[4];
|
||||
b3Float4 m_worldNormalOnB; // w: m_nPoints
|
||||
unsigned short m_restituitionCoeffCmp;
|
||||
unsigned short m_frictionCoeffCmp;
|
||||
int m_batchIdx;
|
||||
int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr
|
||||
int m_bodyBPtrAndSignBit;
|
||||
|
||||
int m_childIndexA;
|
||||
int m_childIndexB;
|
||||
int m_unused1;
|
||||
int m_unused2;
|
||||
|
||||
|
||||
};
|
||||
|
||||
inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
|
||||
{
|
||||
return (int)contact->m_worldNormalOnB.w;
|
||||
};
|
||||
|
||||
inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
|
||||
{
|
||||
contact->m_worldNormalOnB.w = (float)numPoints;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //B3_CONTACT4DATA_H
|
520
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
vendored
Normal file
520
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
vendored
Normal file
|
@ -0,0 +1,520 @@
|
|||
|
||||
#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
#define B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
|
||||
|
||||
#define B3_MAX_VERTS 1024
|
||||
|
||||
|
||||
|
||||
inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t)
|
||||
{
|
||||
return b3MakeVector3( a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
}
|
||||
|
||||
|
||||
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
||||
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut)
|
||||
{
|
||||
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
if (numVertsIn < 2)
|
||||
return 0;
|
||||
|
||||
b3Float4 firstVertex=pVtxIn[numVertsIn-1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex=pVtxIn[ve];
|
||||
|
||||
de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
|
||||
|
||||
if (ds<0)
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
firstVertex = endVertex;
|
||||
ds = de;
|
||||
}
|
||||
return numVertsOut;
|
||||
}
|
||||
|
||||
|
||||
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
|
||||
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
|
||||
b3Float4* worldVertsB2, int capacityWorldVertsB2,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
//const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
|
||||
b3Float4* pVtxIn = worldVertsB1;
|
||||
b3Float4* pVtxOut = worldVertsB2;
|
||||
|
||||
int numVertsIn = numWorldVertsB1;
|
||||
int numVertsOut = 0;
|
||||
|
||||
int closestFaceA=-1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for(int face=0;face<hullA->m_numFaces;face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeVector3(
|
||||
facesA[hullA->m_faceOffset+face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.z,0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
|
||||
|
||||
float d = b3Dot3F4(faceANormalWS,separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (closestFaceA<0)
|
||||
return numContactsOut;
|
||||
|
||||
b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA];
|
||||
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
//int numContacts = numWorldVertsB1;
|
||||
int numVerticesA = polyA.m_numIndices;
|
||||
for(int e0=0;e0<numVerticesA;e0++)
|
||||
{
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];
|
||||
const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];
|
||||
const b3Float4 edge0 = a - b;
|
||||
const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0);
|
||||
b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
|
||||
b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA);
|
||||
|
||||
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
|
||||
float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
|
||||
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS=planeEqWS1;
|
||||
|
||||
//clip face
|
||||
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
|
||||
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
|
||||
|
||||
//btSwap(pVtxIn,pVtxOut);
|
||||
b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsIn = numVertsOut;
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
|
||||
// only keep points that are behind the witness face
|
||||
{
|
||||
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
|
||||
float localPlaneEq = polyA.m_plane.w;
|
||||
b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal);
|
||||
float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA);
|
||||
for (int i=0;i<numVertsIn;i++)
|
||||
{
|
||||
float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS;
|
||||
if (depth <=minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
if (numContactsOut<contactCapacity)
|
||||
{
|
||||
if (depth <=maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
//resultOut.addContactPoint(separatingNormal,point,depth);
|
||||
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
|
||||
//printf("depth=%f\n",depth);
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
|
||||
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB,
|
||||
b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
B3_PROFILE("clipHullAgainstHull");
|
||||
|
||||
//float curMaxDist=maxDist;
|
||||
int closestFaceB=-1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
//B3_PROFILE("closestFaceB");
|
||||
if (hullB.m_numFaces!=1)
|
||||
{
|
||||
//printf("wtf\n");
|
||||
}
|
||||
static bool once = true;
|
||||
//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
|
||||
|
||||
for(int face=0;face<hullB.m_numFaces;face++)
|
||||
{
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("face %d\n",face);
|
||||
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
|
||||
if (once)
|
||||
{
|
||||
for (int i=0;i<faceB->m_numIndices;i++)
|
||||
{
|
||||
b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]];
|
||||
printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z);
|
||||
}
|
||||
}
|
||||
#endif //BT_DEBUG_SAT_FACE
|
||||
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x,
|
||||
facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f);
|
||||
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
|
||||
#endif
|
||||
float d = b3Dot3F4(WorldNormal,separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
once = false;
|
||||
}
|
||||
|
||||
|
||||
b3Assert(closestFaceB>=0);
|
||||
{
|
||||
//B3_PROFILE("worldVertsB1");
|
||||
const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for(int e0=0;e0<numVertices;e0++)
|
||||
{
|
||||
const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
|
||||
worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
|
||||
}
|
||||
}
|
||||
|
||||
if (closestFaceB>=0)
|
||||
{
|
||||
//B3_PROFILE("clipFaceAgainstHull");
|
||||
numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
|
||||
posA,ornA,
|
||||
worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,
|
||||
verticesA, facesA, indicesA,
|
||||
contactsOut,contactCapacity);
|
||||
}
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
inline int b3ClipHullHullSingle(
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
const b3Float4& posA,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB,
|
||||
const b3Quaternion& ornB,
|
||||
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
|
||||
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
|
||||
b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
|
||||
int& nContacts,
|
||||
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
|
||||
const b3Vector3& sepNormalWorldSpace,
|
||||
int maxContactCapacity )
|
||||
{
|
||||
int contactIndex = -1;
|
||||
b3ConvexPolyhedronData hullA, hullB;
|
||||
|
||||
b3Collidable colA = hostCollidablesA[collidableIndexA];
|
||||
hullA = hostConvexDataA[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
|
||||
b3Collidable colB = hostCollidablesB[collidableIndexB];
|
||||
hullB = hostConvexDataB[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
|
||||
b3Float4 contactsOut[B3_MAX_VERTS];
|
||||
int localContactCapacity = B3_MAX_VERTS;
|
||||
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
|
||||
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
|
||||
#endif
|
||||
|
||||
|
||||
{
|
||||
|
||||
b3Float4 worldVertsB1[B3_MAX_VERTS];
|
||||
b3Float4 worldVertsB2[B3_MAX_VERTS];
|
||||
int capacityWorldVerts = B3_MAX_VERTS;
|
||||
|
||||
b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
|
||||
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
|
||||
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
|
||||
|
||||
b3Scalar minDist = -1;
|
||||
b3Scalar maxDist = 0.;
|
||||
|
||||
|
||||
|
||||
b3Transform trA,trB;
|
||||
{
|
||||
//B3_PROFILE("b3TransformPoint computation");
|
||||
//trA.setIdentity();
|
||||
trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
|
||||
trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
|
||||
|
||||
//trB.setIdentity();
|
||||
trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
|
||||
trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
|
||||
}
|
||||
|
||||
b3Quaternion trAorn = trA.getRotation();
|
||||
b3Quaternion trBorn = trB.getRotation();
|
||||
|
||||
int numContactsOut = b3ClipHullAgainstHull(hostNormal,
|
||||
hostConvexDataA.at(shapeA),
|
||||
hostConvexDataB.at(shapeB),
|
||||
(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
|
||||
(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
|
||||
worldVertsB1,worldVertsB2,capacityWorldVerts,
|
||||
minDist, maxDist,
|
||||
verticesA, facesA,indicesA,
|
||||
verticesB, facesB,indicesB,
|
||||
|
||||
contactsOut,localContactCapacity);
|
||||
|
||||
if (numContactsOut>0)
|
||||
{
|
||||
B3_PROFILE("overlap");
|
||||
|
||||
b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
|
||||
// b3Float4 centerOut;
|
||||
|
||||
b3Int4 contactIdx;
|
||||
contactIdx.x = 0;
|
||||
contactIdx.y = 1;
|
||||
contactIdx.z = 2;
|
||||
contactIdx.w = 3;
|
||||
|
||||
int numPoints = 0;
|
||||
|
||||
{
|
||||
B3_PROFILE("extractManifold");
|
||||
numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
|
||||
}
|
||||
|
||||
b3Assert(numPoints);
|
||||
|
||||
if (nContacts<maxContactCapacity)
|
||||
{
|
||||
contactIndex = nContacts;
|
||||
globalContactOut->expand();
|
||||
b3Contact4Data& contact = globalContactOut->at(nContacts);
|
||||
contact.m_batchIdx = 0;//i;
|
||||
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
|
||||
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
|
||||
|
||||
contact.m_frictionCoeffCmp = 45874;
|
||||
contact.m_restituitionCoeffCmp = 0;
|
||||
|
||||
// float distance = 0.f;
|
||||
for (int p=0;p<numPoints;p++)
|
||||
{
|
||||
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
|
||||
contact.m_worldNormalOnB = normalOnSurfaceB;
|
||||
}
|
||||
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
|
||||
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
nContacts++;
|
||||
} else
|
||||
{
|
||||
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline int b3ContactConvexConvexSAT(
|
||||
int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3Collidable>& collidables,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
||||
const b3AlignedObjectArray<b3Float4>& convexVertices,
|
||||
const b3AlignedObjectArray<b3Float4>& uniqueEdges,
|
||||
const b3AlignedObjectArray<int>& convexIndices,
|
||||
const b3AlignedObjectArray<b3GpuFace>& faces,
|
||||
b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
int contactIndex = -1;
|
||||
|
||||
|
||||
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
|
||||
b3ConvexPolyhedronData hullA, hullB;
|
||||
|
||||
b3Float4 sepNormalWorldSpace;
|
||||
|
||||
|
||||
|
||||
b3Collidable colA = collidables[collidableIndexA];
|
||||
hullA = convexShapes[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
|
||||
b3Collidable colB = collidables[collidableIndexB];
|
||||
hullB = convexShapes[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
|
||||
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
|
||||
#endif
|
||||
|
||||
bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB,
|
||||
posA,
|
||||
ornA,
|
||||
posB,
|
||||
ornB,
|
||||
|
||||
convexVertices,uniqueEdges,faces,convexIndices,
|
||||
convexVertices,uniqueEdges,faces,convexIndices,
|
||||
|
||||
sepNormalWorldSpace
|
||||
);
|
||||
|
||||
|
||||
if (foundSepAxis)
|
||||
{
|
||||
|
||||
|
||||
contactIndex = b3ClipHullHullSingle(
|
||||
bodyIndexA, bodyIndexB,
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
collidableIndexA, collidableIndexB,
|
||||
&rigidBodies,
|
||||
&globalContactsOut,
|
||||
nGlobalContactsOut,
|
||||
|
||||
convexShapes,
|
||||
convexShapes,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
collidables,
|
||||
collidables,
|
||||
sepNormalWorldSpace,
|
||||
maxContactCapacity);
|
||||
|
||||
}
|
||||
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
|
162
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
vendored
Normal file
162
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
vendored
Normal file
|
@ -0,0 +1,162 @@
|
|||
|
||||
#ifndef B3_CONTACT_SPHERE_SPHERE_H
|
||||
#define B3_CONTACT_SPHERE_SPHERE_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void computeContactSphereConvex(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3RigidBodyData* rigidBodies,
|
||||
const b3Collidable* collidables,
|
||||
const b3ConvexPolyhedronData* convexShapes,
|
||||
const b3Vector3* convexVertices,
|
||||
const int* convexIndices,
|
||||
const b3GpuFace* faces,
|
||||
b3Contact4* globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
|
||||
float radius = collidables[collidableIndexA].m_radius;
|
||||
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
|
||||
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
|
||||
|
||||
|
||||
|
||||
float4 pos = rigidBodies[bodyIndexB].m_pos;
|
||||
|
||||
|
||||
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
b3Transform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(pos);
|
||||
tr.setRotation(quat);
|
||||
b3Transform trInv = tr.inverse();
|
||||
|
||||
float4 spherePos = trInv(spherePos1);
|
||||
|
||||
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
int numFaces = convexShapes[shapeIndex].m_numFaces;
|
||||
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
|
||||
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
|
||||
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
|
||||
bool bCollide = true;
|
||||
int region = -1;
|
||||
float4 localHitNormal;
|
||||
for ( int f = 0; f < numFaces; f++ )
|
||||
{
|
||||
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
|
||||
float4 planeEqn;
|
||||
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
|
||||
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
|
||||
planeEqn = n1;
|
||||
planeEqn[3] = face.m_plane.w;
|
||||
|
||||
float4 pntReturn;
|
||||
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
|
||||
|
||||
if ( dist > radius)
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if ( dist > 0 )
|
||||
{
|
||||
//might hit an edge or vertex
|
||||
b3Vector3 out;
|
||||
|
||||
bool isInPoly = IsPointInPolygon(spherePos,
|
||||
&face,
|
||||
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
|
||||
convexIndices,
|
||||
&out);
|
||||
if (isInPoly)
|
||||
{
|
||||
if (dist>minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region=1;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Vector3 tmp = spherePos-out;
|
||||
b3Scalar l2 = tmp.length2();
|
||||
if (l2<radius*radius)
|
||||
{
|
||||
dist = b3Sqrt(l2);
|
||||
if (dist>minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = out;
|
||||
localHitNormal = tmp/dist;
|
||||
region=2;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( dist > minDist )
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region=3;
|
||||
}
|
||||
}
|
||||
}
|
||||
static int numChecks = 0;
|
||||
numChecks++;
|
||||
|
||||
if (bCollide && minDist > -10000)
|
||||
{
|
||||
|
||||
float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
|
||||
float4 pOnB1 = tr(closestPnt);
|
||||
//printf("dist ,%f,",minDist);
|
||||
float actualDepth = minDist-radius;
|
||||
if (actualDepth<0)
|
||||
{
|
||||
//printf("actualDepth = ,%f,", actualDepth);
|
||||
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
|
||||
//printf("region=,%d,\n", region);
|
||||
pOnB1[3] = actualDepth;
|
||||
|
||||
int dstIdx;
|
||||
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
||||
|
||||
if (nGlobalContactsOut < maxContactCapacity)
|
||||
{
|
||||
dstIdx=nGlobalContactsOut;
|
||||
nGlobalContactsOut++;
|
||||
|
||||
b3Contact4* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = normalOnSurfaceB1;
|
||||
c->setFrictionCoeff(0.7);
|
||||
c->setRestituitionCoeff(0.f);
|
||||
|
||||
c->m_batchIdx = pairIndex;
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
|
||||
c->m_worldPosB[0] = pOnB1;
|
||||
int numPoints = 1;
|
||||
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
}//if (dstIdx < numPairs)
|
||||
}
|
||||
}//if (hasCollision)
|
||||
|
||||
}
|
||||
#endif //B3_CONTACT_SPHERE_SPHERE_H
|
40
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
vendored
Normal file
40
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
vendored
Normal file
|
@ -0,0 +1,40 @@
|
|||
|
||||
#ifndef B3_CONVEX_POLYHEDRON_DATA_H
|
||||
#define B3_CONVEX_POLYHEDRON_DATA_H
|
||||
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
typedef struct b3GpuFace b3GpuFace_t;
|
||||
struct b3GpuFace
|
||||
{
|
||||
b3Float4 m_plane;
|
||||
int m_indexOffset;
|
||||
int m_numIndices;
|
||||
int m_unusedPadding1;
|
||||
int m_unusedPadding2;
|
||||
};
|
||||
|
||||
typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
|
||||
|
||||
struct b3ConvexPolyhedronData
|
||||
{
|
||||
b3Float4 m_localCenter;
|
||||
b3Float4 m_extents;
|
||||
b3Float4 mC;
|
||||
b3Float4 mE;
|
||||
|
||||
float m_radius;
|
||||
int m_faceOffset;
|
||||
int m_numFaces;
|
||||
int m_numVertices;
|
||||
|
||||
int m_vertexOffset;
|
||||
int m_uniqueEdgesOffset;
|
||||
int m_numUniqueEdges;
|
||||
int m_unused;
|
||||
};
|
||||
|
||||
#endif //B3_CONVEX_POLYHEDRON_DATA_H
|
832
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
vendored
Normal file
832
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
vendored
Normal file
|
@ -0,0 +1,832 @@
|
|||
#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
|
||||
#define B3_TRIANGLE_NUM_CONVEX_FACES 5
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
|
||||
inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
|
||||
const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
|
||||
{
|
||||
min[0] = FLT_MAX;
|
||||
max[0] = -FLT_MAX;
|
||||
int numVerts = hull->m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir);
|
||||
float offset = b3Dot(pos,*dir);
|
||||
for(int i=0;i<numVerts;i++)
|
||||
{
|
||||
float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir);
|
||||
if(dp < min[0])
|
||||
min[0] = dp;
|
||||
if(dp > max[0])
|
||||
max[0] = dp;
|
||||
}
|
||||
if(min[0]>max[0])
|
||||
{
|
||||
float tmp = min[0];
|
||||
min[0] = max[0];
|
||||
max[0] = tmp;
|
||||
}
|
||||
min[0] += offset;
|
||||
max[0] += offset;
|
||||
}
|
||||
|
||||
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA,b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB,b3QuatConstArg ornB,
|
||||
b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth)
|
||||
{
|
||||
float Min0,Max0;
|
||||
float Min1,Max1;
|
||||
b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);
|
||||
b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
*depth = d0<d1 ? d0:d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA1,
|
||||
b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB1,
|
||||
b3QuatConstArg ornB,
|
||||
b3Float4ConstArg DeltaC2,
|
||||
|
||||
const b3Float4* verticesA,
|
||||
const b3Float4* uniqueEdgesA,
|
||||
const b3GpuFace* facesA,
|
||||
const int* indicesA,
|
||||
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3Float4* uniqueEdgesB,
|
||||
__global const b3GpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
b3Float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
/*
|
||||
static int maxFaceVertex = 0;
|
||||
|
||||
int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
|
||||
curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
|
||||
|
||||
if (curFaceVertexAB>maxFaceVertex)
|
||||
{
|
||||
maxFaceVertex = curFaceVertexAB;
|
||||
printf("curFaceVertexAB = %d\n",curFaceVertexAB);
|
||||
printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
|
||||
printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
|
||||
printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
|
||||
}
|
||||
*/
|
||||
|
||||
int curPlaneTests=0;
|
||||
{
|
||||
int numFacesA = hullA->m_numFaces;
|
||||
// Test normals from hullA
|
||||
for(int i=0;i<numFacesA;i++)
|
||||
{
|
||||
const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
|
||||
if (b3Dot(DeltaC2,faceANormalWS)<0)
|
||||
faceANormalWS*=-1.f;
|
||||
curPlaneTests++;
|
||||
float d;
|
||||
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))
|
||||
return false;
|
||||
if(d<*dmin)
|
||||
{
|
||||
*dmin = d;
|
||||
*sep = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
if((b3Dot(-DeltaC2,*sep))>0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
b3Vector3 unitSphere162[]=
|
||||
{
|
||||
b3MakeVector3(0.000000,-1.000000,0.000000),
|
||||
b3MakeVector3(0.203181,-0.967950,0.147618),
|
||||
b3MakeVector3(-0.077607,-0.967950,0.238853),
|
||||
b3MakeVector3(0.723607,-0.447220,0.525725),
|
||||
b3MakeVector3(0.609547,-0.657519,0.442856),
|
||||
b3MakeVector3(0.812729,-0.502301,0.295238),
|
||||
b3MakeVector3(-0.251147,-0.967949,0.000000),
|
||||
b3MakeVector3(-0.077607,-0.967950,-0.238853),
|
||||
b3MakeVector3(0.203181,-0.967950,-0.147618),
|
||||
b3MakeVector3(0.860698,-0.251151,0.442858),
|
||||
b3MakeVector3(-0.276388,-0.447220,0.850649),
|
||||
b3MakeVector3(-0.029639,-0.502302,0.864184),
|
||||
b3MakeVector3(-0.155215,-0.251152,0.955422),
|
||||
b3MakeVector3(-0.894426,-0.447216,0.000000),
|
||||
b3MakeVector3(-0.831051,-0.502299,0.238853),
|
||||
b3MakeVector3(-0.956626,-0.251149,0.147618),
|
||||
b3MakeVector3(-0.276388,-0.447220,-0.850649),
|
||||
b3MakeVector3(-0.483971,-0.502302,-0.716565),
|
||||
b3MakeVector3(-0.436007,-0.251152,-0.864188),
|
||||
b3MakeVector3(0.723607,-0.447220,-0.525725),
|
||||
b3MakeVector3(0.531941,-0.502302,-0.681712),
|
||||
b3MakeVector3(0.687159,-0.251152,-0.681715),
|
||||
b3MakeVector3(0.687159,-0.251152,0.681715),
|
||||
b3MakeVector3(-0.436007,-0.251152,0.864188),
|
||||
b3MakeVector3(-0.956626,-0.251149,-0.147618),
|
||||
b3MakeVector3(-0.155215,-0.251152,-0.955422),
|
||||
b3MakeVector3(0.860698,-0.251151,-0.442858),
|
||||
b3MakeVector3(0.276388,0.447220,0.850649),
|
||||
b3MakeVector3(0.483971,0.502302,0.716565),
|
||||
b3MakeVector3(0.232822,0.657519,0.716563),
|
||||
b3MakeVector3(-0.723607,0.447220,0.525725),
|
||||
b3MakeVector3(-0.531941,0.502302,0.681712),
|
||||
b3MakeVector3(-0.609547,0.657519,0.442856),
|
||||
b3MakeVector3(-0.723607,0.447220,-0.525725),
|
||||
b3MakeVector3(-0.812729,0.502301,-0.295238),
|
||||
b3MakeVector3(-0.609547,0.657519,-0.442856),
|
||||
b3MakeVector3(0.276388,0.447220,-0.850649),
|
||||
b3MakeVector3(0.029639,0.502302,-0.864184),
|
||||
b3MakeVector3(0.232822,0.657519,-0.716563),
|
||||
b3MakeVector3(0.894426,0.447216,0.000000),
|
||||
b3MakeVector3(0.831051,0.502299,-0.238853),
|
||||
b3MakeVector3(0.753442,0.657515,0.000000),
|
||||
b3MakeVector3(-0.232822,-0.657519,0.716563),
|
||||
b3MakeVector3(-0.162456,-0.850654,0.499995),
|
||||
b3MakeVector3(0.052790,-0.723612,0.688185),
|
||||
b3MakeVector3(0.138199,-0.894429,0.425321),
|
||||
b3MakeVector3(0.262869,-0.525738,0.809012),
|
||||
b3MakeVector3(0.361805,-0.723611,0.587779),
|
||||
b3MakeVector3(0.531941,-0.502302,0.681712),
|
||||
b3MakeVector3(0.425323,-0.850654,0.309011),
|
||||
b3MakeVector3(0.812729,-0.502301,-0.295238),
|
||||
b3MakeVector3(0.609547,-0.657519,-0.442856),
|
||||
b3MakeVector3(0.850648,-0.525736,0.000000),
|
||||
b3MakeVector3(0.670817,-0.723611,-0.162457),
|
||||
b3MakeVector3(0.670817,-0.723610,0.162458),
|
||||
b3MakeVector3(0.425323,-0.850654,-0.309011),
|
||||
b3MakeVector3(0.447211,-0.894428,0.000001),
|
||||
b3MakeVector3(-0.753442,-0.657515,0.000000),
|
||||
b3MakeVector3(-0.525730,-0.850652,0.000000),
|
||||
b3MakeVector3(-0.638195,-0.723609,0.262864),
|
||||
b3MakeVector3(-0.361801,-0.894428,0.262864),
|
||||
b3MakeVector3(-0.688189,-0.525736,0.499997),
|
||||
b3MakeVector3(-0.447211,-0.723610,0.525729),
|
||||
b3MakeVector3(-0.483971,-0.502302,0.716565),
|
||||
b3MakeVector3(-0.232822,-0.657519,-0.716563),
|
||||
b3MakeVector3(-0.162456,-0.850654,-0.499995),
|
||||
b3MakeVector3(-0.447211,-0.723611,-0.525727),
|
||||
b3MakeVector3(-0.361801,-0.894429,-0.262863),
|
||||
b3MakeVector3(-0.688189,-0.525736,-0.499997),
|
||||
b3MakeVector3(-0.638195,-0.723609,-0.262863),
|
||||
b3MakeVector3(-0.831051,-0.502299,-0.238853),
|
||||
b3MakeVector3(0.361804,-0.723612,-0.587779),
|
||||
b3MakeVector3(0.138197,-0.894429,-0.425321),
|
||||
b3MakeVector3(0.262869,-0.525738,-0.809012),
|
||||
b3MakeVector3(0.052789,-0.723611,-0.688186),
|
||||
b3MakeVector3(-0.029639,-0.502302,-0.864184),
|
||||
b3MakeVector3(0.956626,0.251149,0.147618),
|
||||
b3MakeVector3(0.956626,0.251149,-0.147618),
|
||||
b3MakeVector3(0.951058,-0.000000,0.309013),
|
||||
b3MakeVector3(1.000000,0.000000,0.000000),
|
||||
b3MakeVector3(0.947213,-0.276396,0.162458),
|
||||
b3MakeVector3(0.951058,0.000000,-0.309013),
|
||||
b3MakeVector3(0.947213,-0.276396,-0.162458),
|
||||
b3MakeVector3(0.155215,0.251152,0.955422),
|
||||
b3MakeVector3(0.436007,0.251152,0.864188),
|
||||
b3MakeVector3(-0.000000,-0.000000,1.000000),
|
||||
b3MakeVector3(0.309017,0.000000,0.951056),
|
||||
b3MakeVector3(0.138199,-0.276398,0.951055),
|
||||
b3MakeVector3(0.587786,0.000000,0.809017),
|
||||
b3MakeVector3(0.447216,-0.276398,0.850648),
|
||||
b3MakeVector3(-0.860698,0.251151,0.442858),
|
||||
b3MakeVector3(-0.687159,0.251152,0.681715),
|
||||
b3MakeVector3(-0.951058,-0.000000,0.309013),
|
||||
b3MakeVector3(-0.809018,0.000000,0.587783),
|
||||
b3MakeVector3(-0.861803,-0.276396,0.425324),
|
||||
b3MakeVector3(-0.587786,0.000000,0.809017),
|
||||
b3MakeVector3(-0.670819,-0.276397,0.688191),
|
||||
b3MakeVector3(-0.687159,0.251152,-0.681715),
|
||||
b3MakeVector3(-0.860698,0.251151,-0.442858),
|
||||
b3MakeVector3(-0.587786,-0.000000,-0.809017),
|
||||
b3MakeVector3(-0.809018,-0.000000,-0.587783),
|
||||
b3MakeVector3(-0.670819,-0.276397,-0.688191),
|
||||
b3MakeVector3(-0.951058,0.000000,-0.309013),
|
||||
b3MakeVector3(-0.861803,-0.276396,-0.425324),
|
||||
b3MakeVector3(0.436007,0.251152,-0.864188),
|
||||
b3MakeVector3(0.155215,0.251152,-0.955422),
|
||||
b3MakeVector3(0.587786,-0.000000,-0.809017),
|
||||
b3MakeVector3(0.309017,-0.000000,-0.951056),
|
||||
b3MakeVector3(0.447216,-0.276398,-0.850648),
|
||||
b3MakeVector3(0.000000,0.000000,-1.000000),
|
||||
b3MakeVector3(0.138199,-0.276398,-0.951055),
|
||||
b3MakeVector3(0.670820,0.276396,0.688190),
|
||||
b3MakeVector3(0.809019,-0.000002,0.587783),
|
||||
b3MakeVector3(0.688189,0.525736,0.499997),
|
||||
b3MakeVector3(0.861804,0.276394,0.425323),
|
||||
b3MakeVector3(0.831051,0.502299,0.238853),
|
||||
b3MakeVector3(-0.447216,0.276397,0.850649),
|
||||
b3MakeVector3(-0.309017,-0.000001,0.951056),
|
||||
b3MakeVector3(-0.262869,0.525738,0.809012),
|
||||
b3MakeVector3(-0.138199,0.276397,0.951055),
|
||||
b3MakeVector3(0.029639,0.502302,0.864184),
|
||||
b3MakeVector3(-0.947213,0.276396,-0.162458),
|
||||
b3MakeVector3(-1.000000,0.000001,0.000000),
|
||||
b3MakeVector3(-0.850648,0.525736,-0.000000),
|
||||
b3MakeVector3(-0.947213,0.276397,0.162458),
|
||||
b3MakeVector3(-0.812729,0.502301,0.295238),
|
||||
b3MakeVector3(-0.138199,0.276397,-0.951055),
|
||||
b3MakeVector3(-0.309016,-0.000000,-0.951057),
|
||||
b3MakeVector3(-0.262869,0.525738,-0.809012),
|
||||
b3MakeVector3(-0.447215,0.276397,-0.850649),
|
||||
b3MakeVector3(-0.531941,0.502302,-0.681712),
|
||||
b3MakeVector3(0.861804,0.276396,-0.425322),
|
||||
b3MakeVector3(0.809019,0.000000,-0.587782),
|
||||
b3MakeVector3(0.688189,0.525736,-0.499997),
|
||||
b3MakeVector3(0.670821,0.276397,-0.688189),
|
||||
b3MakeVector3(0.483971,0.502302,-0.716565),
|
||||
b3MakeVector3(0.077607,0.967950,0.238853),
|
||||
b3MakeVector3(0.251147,0.967949,0.000000),
|
||||
b3MakeVector3(0.000000,1.000000,0.000000),
|
||||
b3MakeVector3(0.162456,0.850654,0.499995),
|
||||
b3MakeVector3(0.361800,0.894429,0.262863),
|
||||
b3MakeVector3(0.447209,0.723612,0.525728),
|
||||
b3MakeVector3(0.525730,0.850652,0.000000),
|
||||
b3MakeVector3(0.638194,0.723610,0.262864),
|
||||
b3MakeVector3(-0.203181,0.967950,0.147618),
|
||||
b3MakeVector3(-0.425323,0.850654,0.309011),
|
||||
b3MakeVector3(-0.138197,0.894430,0.425320),
|
||||
b3MakeVector3(-0.361804,0.723612,0.587778),
|
||||
b3MakeVector3(-0.052790,0.723612,0.688185),
|
||||
b3MakeVector3(-0.203181,0.967950,-0.147618),
|
||||
b3MakeVector3(-0.425323,0.850654,-0.309011),
|
||||
b3MakeVector3(-0.447210,0.894429,0.000000),
|
||||
b3MakeVector3(-0.670817,0.723611,-0.162457),
|
||||
b3MakeVector3(-0.670817,0.723611,0.162457),
|
||||
b3MakeVector3(0.077607,0.967950,-0.238853),
|
||||
b3MakeVector3(0.162456,0.850654,-0.499995),
|
||||
b3MakeVector3(-0.138197,0.894430,-0.425320),
|
||||
b3MakeVector3(-0.052790,0.723612,-0.688185),
|
||||
b3MakeVector3(-0.361804,0.723612,-0.587778),
|
||||
b3MakeVector3(0.361800,0.894429,-0.262863),
|
||||
b3MakeVector3(0.638194,0.723610,-0.262864),
|
||||
b3MakeVector3(0.447209,0.723612,-0.525728)
|
||||
};
|
||||
|
||||
|
||||
bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA1,
|
||||
b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB1,
|
||||
b3QuatConstArg ornB,
|
||||
b3Float4ConstArg DeltaC2,
|
||||
const b3Float4* verticesA,
|
||||
const b3Float4* uniqueEdgesA,
|
||||
const b3GpuFace* facesA,
|
||||
const int* indicesA,
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3Float4* uniqueEdgesB,
|
||||
__global const b3GpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
b3Float4* sep,
|
||||
float* dmin,
|
||||
bool searchAllEdgeEdge)
|
||||
{
|
||||
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
|
||||
// int curPlaneTests=0;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
static int maxEdgeTests = 0;
|
||||
int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
|
||||
if (curEdgeTests >maxEdgeTests )
|
||||
{
|
||||
maxEdgeTests = curEdgeTests ;
|
||||
printf("maxEdgeTests = %d\n",maxEdgeTests );
|
||||
printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges);
|
||||
printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges);
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (searchAllEdgeEdge)
|
||||
{
|
||||
for(int e0=0;e0<hullA->m_numUniqueEdges;e0++)
|
||||
{
|
||||
const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA,edge0);
|
||||
|
||||
for(int e1=0;e1<hullB->m_numUniqueEdges;e1++)
|
||||
{
|
||||
const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB,edge1);
|
||||
|
||||
|
||||
b3Float4 crossje = b3Cross(edge0World,edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if(!b3IsAlmostZero(crossje))
|
||||
{
|
||||
crossje = b3Normalized(crossje);
|
||||
if (b3Dot(DeltaC2,crossje)<0)
|
||||
crossje *= -1.f;
|
||||
|
||||
float dist;
|
||||
bool result = true;
|
||||
{
|
||||
float Min0,Max0;
|
||||
float Min1,Max1;
|
||||
b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);
|
||||
b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
if(dist<*dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3);
|
||||
//printf("numDirections =%d\n",numDirections );
|
||||
|
||||
|
||||
for(int i=0;i<numDirections;i++)
|
||||
{
|
||||
b3Float4 crossje = unitSphere162[i];
|
||||
{
|
||||
//if (b3Dot(DeltaC2,crossje)>0)
|
||||
{
|
||||
float dist;
|
||||
bool result = true;
|
||||
{
|
||||
float Min0,Max0;
|
||||
float Min1,Max1;
|
||||
b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);
|
||||
b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
if(dist<*dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if((b3Dot(-DeltaC2,*sep))>0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
|
||||
__global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
|
||||
b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
__global const b3Float4* verticesA,
|
||||
__global const b3GpuFace_t* facesA,
|
||||
__global const int* indicesA,
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3GpuFace_t* facesB,
|
||||
__global const int* indicesB,
|
||||
|
||||
__global b3Int4* clippingFaces, int pairIndex)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
|
||||
int closestFaceB=-1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
for(int face=0;face<hullB->m_numFaces;face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x,
|
||||
facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
|
||||
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
float d = b3Dot(WorldNormal,separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for(int e0=0;e0<numVertices;e0++)
|
||||
{
|
||||
const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
|
||||
worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
|
||||
}
|
||||
}
|
||||
|
||||
int closestFaceA=-1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for(int face=0;face<hullA->m_numFaces;face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeFloat4(
|
||||
facesA[hullA->m_faceOffset+face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.z,
|
||||
0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
|
||||
|
||||
float d = b3Dot(faceANormalWS,separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
worldNormalsA1[pairIndex] = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
|
||||
for(int e0=0;e0<numVerticesA;e0++)
|
||||
{
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
|
||||
worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA);
|
||||
}
|
||||
|
||||
clippingFaces[pairIndex].x = closestFaceA;
|
||||
clippingFaces[pairIndex].y = closestFaceB;
|
||||
clippingFaces[pairIndex].z = numVerticesA;
|
||||
clippingFaces[pairIndex].w = numWorldVertsB1;
|
||||
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global const b3ConvexPolyhedronData* convexShapes,
|
||||
__global const b3Float4* vertices,
|
||||
__global const b3Float4* uniqueEdges,
|
||||
__global const b3GpuFace* faces,
|
||||
__global const int* indices,
|
||||
__global const b3GpuChildShape* gpuChildShapes,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Float4* concaveSeparatingNormalsOut,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Vector3* worldVertsA1Out,
|
||||
__global b3Vector3* worldNormalsA1Out,
|
||||
__global b3Vector3* worldVertsB1Out,
|
||||
__global int* hasSeparatingNormals,
|
||||
int vertexFaceCapacity,
|
||||
int numConcavePairs,
|
||||
int pairIdx
|
||||
)
|
||||
{
|
||||
int i = pairIdx;
|
||||
/* int i = get_global_id(0);
|
||||
if (i>=numConcavePairs)
|
||||
return;
|
||||
int pairIdx = i;
|
||||
*/
|
||||
|
||||
int bodyIndexA = concavePairs[i].x;
|
||||
int bodyIndexB = concavePairs[i].y;
|
||||
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
||||
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
||||
|
||||
if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&
|
||||
collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
concavePairs[pairIdx].w = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
hasSeparatingNormals[i] = 0;
|
||||
|
||||
// int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
||||
int numActualConcaveConvexTests = 0;
|
||||
|
||||
int f = concavePairs[i].z;
|
||||
|
||||
bool overlap = false;
|
||||
|
||||
b3ConvexPolyhedronData convexPolyhedronA;
|
||||
|
||||
//add 3 vertices of the triangle
|
||||
convexPolyhedronA.m_numVertices = 3;
|
||||
convexPolyhedronA.m_vertexOffset = 0;
|
||||
b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f);
|
||||
|
||||
b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
|
||||
b3Aabb triAabb;
|
||||
triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f);
|
||||
triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f);
|
||||
|
||||
b3Float4 verticesA[3];
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
int index = indices[face.m_indexOffset+i];
|
||||
b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
|
||||
verticesA[i] = vert;
|
||||
localCenter += vert;
|
||||
|
||||
triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert);
|
||||
triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert);
|
||||
|
||||
}
|
||||
|
||||
overlap = true;
|
||||
overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
|
||||
overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
|
||||
overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
|
||||
|
||||
if (overlap)
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
int hasSeparatingAxis=5;
|
||||
b3Float4 sepAxis=b3MakeFloat4(1,2,3,4);
|
||||
|
||||
// int localCC=0;
|
||||
numActualConcaveConvexTests++;
|
||||
|
||||
//a triangle has 3 unique edges
|
||||
convexPolyhedronA.m_numUniqueEdges = 3;
|
||||
convexPolyhedronA.m_uniqueEdgesOffset = 0;
|
||||
b3Float4 uniqueEdgesA[3];
|
||||
|
||||
uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);
|
||||
uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);
|
||||
uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);
|
||||
|
||||
|
||||
convexPolyhedronA.m_faceOffset = 0;
|
||||
|
||||
b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
|
||||
|
||||
b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
|
||||
int indicesA[3+3+2+2+2];
|
||||
int curUsedIndices=0;
|
||||
int fidx=0;
|
||||
|
||||
//front size of triangle
|
||||
{
|
||||
facesA[fidx].m_indexOffset=curUsedIndices;
|
||||
indicesA[0] = 0;
|
||||
indicesA[1] = 1;
|
||||
indicesA[2] = 2;
|
||||
curUsedIndices+=3;
|
||||
float c = face.m_plane.w;
|
||||
facesA[fidx].m_plane.x = normal.x;
|
||||
facesA[fidx].m_plane.y = normal.y;
|
||||
facesA[fidx].m_plane.z = normal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
facesA[fidx].m_numIndices=3;
|
||||
}
|
||||
fidx++;
|
||||
//back size of triangle
|
||||
{
|
||||
facesA[fidx].m_indexOffset=curUsedIndices;
|
||||
indicesA[3]=2;
|
||||
indicesA[4]=1;
|
||||
indicesA[5]=0;
|
||||
curUsedIndices+=3;
|
||||
float c = b3Dot(normal,verticesA[0]);
|
||||
// float c1 = -face.m_plane.w;
|
||||
facesA[fidx].m_plane.x = -normal.x;
|
||||
facesA[fidx].m_plane.y = -normal.y;
|
||||
facesA[fidx].m_plane.z = -normal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
facesA[fidx].m_numIndices=3;
|
||||
}
|
||||
fidx++;
|
||||
|
||||
bool addEdgePlanes = true;
|
||||
if (addEdgePlanes)
|
||||
{
|
||||
int numVertices=3;
|
||||
int prevVertex = numVertices-1;
|
||||
for (int i=0;i<numVertices;i++)
|
||||
{
|
||||
b3Float4 v0 = verticesA[i];
|
||||
b3Float4 v1 = verticesA[prevVertex];
|
||||
|
||||
b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0));
|
||||
float c = -b3Dot(edgeNormal,v0);
|
||||
|
||||
facesA[fidx].m_numIndices = 2;
|
||||
facesA[fidx].m_indexOffset=curUsedIndices;
|
||||
indicesA[curUsedIndices++]=i;
|
||||
indicesA[curUsedIndices++]=prevVertex;
|
||||
|
||||
facesA[fidx].m_plane.x = edgeNormal.x;
|
||||
facesA[fidx].m_plane.y = edgeNormal.y;
|
||||
facesA[fidx].m_plane.z = edgeNormal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
fidx++;
|
||||
prevVertex = i;
|
||||
}
|
||||
}
|
||||
convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
|
||||
convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f);
|
||||
|
||||
|
||||
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
posB.w = 0.f;
|
||||
|
||||
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
|
||||
|
||||
|
||||
///////////////////
|
||||
///compound shape support
|
||||
|
||||
if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int compoundChild = concavePairs[pairIdx].w;
|
||||
int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild;
|
||||
int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
||||
b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
||||
b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
||||
b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB);
|
||||
b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB);
|
||||
posB = newPosB;
|
||||
ornB = newOrnB;
|
||||
shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
||||
}
|
||||
//////////////////
|
||||
|
||||
b3Float4 c0local = convexPolyhedronA.m_localCenter;
|
||||
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
||||
b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
||||
b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
|
||||
const b3Float4 DeltaC2 = c0 - c1;
|
||||
|
||||
|
||||
bool sepA = b3FindSeparatingAxis( &convexPolyhedronA, &convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
DeltaC2,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
&sepAxis,&dmin);
|
||||
hasSeparatingAxis = 4;
|
||||
if (!sepA)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
bool sepB = b3FindSeparatingAxis( &convexShapes[shapeIndexB],&convexPolyhedronA,
|
||||
posB,ornB,
|
||||
posA,ornA,
|
||||
DeltaC2,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
&sepAxis,&dmin);
|
||||
|
||||
if (!sepB)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
bool sepEE = b3FindSeparatingAxisEdgeEdge( &convexPolyhedronA, &convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
DeltaC2,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
&sepAxis,&dmin,true);
|
||||
|
||||
if (!sepEE)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
hasSeparatingAxis = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (hasSeparatingAxis)
|
||||
{
|
||||
hasSeparatingNormals[i]=1;
|
||||
sepAxis.w = dmin;
|
||||
concaveSeparatingNormalsOut[pairIdx]=sepAxis;
|
||||
|
||||
//now compute clipping faces A and B, and world-space clipping vertices A and B...
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
b3FindClippingFaces(sepAxis,
|
||||
&convexPolyhedronA,
|
||||
&convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
worldVertsA1Out,
|
||||
worldNormalsA1Out,
|
||||
worldVertsB1Out,
|
||||
vertexFaceCapacity,
|
||||
minDist, maxDist,
|
||||
verticesA,
|
||||
facesA,
|
||||
indicesA,
|
||||
|
||||
vertices,
|
||||
faces,
|
||||
indices,
|
||||
clippingFacesOut, pairIdx);
|
||||
|
||||
} else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
concavePairs[pairIdx].w = -1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
concavePairs[pairIdx].w = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
|
206
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
vendored
Normal file
206
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
vendored
Normal file
|
@ -0,0 +1,206 @@
|
|||
#ifndef B3_FIND_SEPARATING_AXIS_H
|
||||
#define B3_FIND_SEPARATING_AXIS_H
|
||||
|
||||
|
||||
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
||||
{
|
||||
min = FLT_MAX;
|
||||
max = -FLT_MAX;
|
||||
int numVerts = hull.m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir);
|
||||
|
||||
b3Scalar offset = b3Dot3F4(pos,dir);
|
||||
|
||||
for(int i=0;i<numVerts;i++)
|
||||
{
|
||||
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
|
||||
//b3Scalar dp = pt.dot(dir);
|
||||
//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
|
||||
b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir);
|
||||
//b3Assert(dp==dpL);
|
||||
if(dp < min) min = dp;
|
||||
if(dp > max) max = dp;
|
||||
}
|
||||
if(min>max)
|
||||
{
|
||||
b3Scalar tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
}
|
||||
min += offset;
|
||||
max += offset;
|
||||
}
|
||||
|
||||
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA,const b3Quaternion& ornA,
|
||||
const b3Float4& posB,const b3Quaternion& ornB,
|
||||
const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
|
||||
{
|
||||
b3Scalar Min0,Max0;
|
||||
b3Scalar Min1,Max1;
|
||||
b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0);
|
||||
b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
b3Scalar d0 = Max0 - Min1;
|
||||
b3Assert(d0>=0.0f);
|
||||
b3Scalar d1 = Max1 - Min0;
|
||||
b3Assert(d1>=0.0f);
|
||||
depth = d0<d1 ? d0:d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA1,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB1,
|
||||
const b3Quaternion& ornB,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
b3Vector3& sep)
|
||||
{
|
||||
B3_PROFILE("findSeparatingAxis");
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
//#ifdef TEST_INTERNAL_OBJECTS
|
||||
b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
|
||||
|
||||
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
||||
b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
|
||||
b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
|
||||
const b3Float4 deltaC2 = c0 - c1;
|
||||
//#endif
|
||||
|
||||
b3Scalar dmin = FLT_MAX;
|
||||
int curPlaneTests=0;
|
||||
|
||||
int numFacesA = hullA.m_numFaces;
|
||||
// Test normals from hullA
|
||||
for(int i=0;i<numFacesA;i++)
|
||||
{
|
||||
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2,faceANormalWS)<0)
|
||||
faceANormalWS*=-1.f;
|
||||
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
|
||||
b3Scalar d;
|
||||
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d))
|
||||
return false;
|
||||
|
||||
if(d<dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)faceANormalWS;
|
||||
}
|
||||
}
|
||||
|
||||
int numFacesB = hullB.m_numFaces;
|
||||
// Test normals from hullB
|
||||
for(int i=0;i<numFacesB;i++)
|
||||
{
|
||||
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane;
|
||||
b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2,WorldNormal)<0)
|
||||
{
|
||||
WorldNormal*=-1.f;
|
||||
}
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar d;
|
||||
if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d))
|
||||
return false;
|
||||
|
||||
if(d<dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)WorldNormal;
|
||||
}
|
||||
}
|
||||
|
||||
// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
|
||||
{
|
||||
const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0);
|
||||
|
||||
for(int e1=0;e1<hullB.m_numUniqueEdges;e1++)
|
||||
{
|
||||
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1);
|
||||
|
||||
|
||||
b3Float4 crossje = b3Cross3(edge0World,edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if(!b3IsAlmostZero((b3Vector3&)crossje))
|
||||
{
|
||||
crossje = b3FastNormalized3(crossje);
|
||||
if (b3Dot3F4(deltaC2,crossje)<0)
|
||||
crossje*=-1.f;
|
||||
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar dist;
|
||||
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist))
|
||||
return false;
|
||||
|
||||
if(dist<dmin)
|
||||
{
|
||||
dmin = dist;
|
||||
sep = (b3Vector3&)crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f)
|
||||
sep = -sep;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif //B3_FIND_SEPARATING_AXIS_H
|
||||
|
920
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
vendored
Normal file
920
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
vendored
Normal file
|
@ -0,0 +1,920 @@
|
|||
|
||||
/***
|
||||
* ---------------------------------
|
||||
* Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
|
||||
*
|
||||
* This file was ported from mpr.c file, part of libccd.
|
||||
* The Minkoski Portal Refinement implementation was ported
|
||||
* to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
|
||||
* at http://github.com/erwincoumans/bullet3
|
||||
*
|
||||
* Distributed under the OSI-approved BSD License (the "License");
|
||||
* see <http://www.opensource.org/licenses/bsd-license.php>.
|
||||
* This software is distributed WITHOUT ANY WARRANTY; without even the
|
||||
* implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the License for more information.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#ifndef B3_MPR_PENETRATION_H
|
||||
#define B3_MPR_PENETRATION_H
|
||||
|
||||
#include "Bullet3Common/shared/b3PlatformDefinitions.h"
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define B3_MPR_SQRT sqrtf
|
||||
#else
|
||||
#define B3_MPR_SQRT sqrt
|
||||
#endif
|
||||
#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
|
||||
#define B3_MPR_FABS fabs
|
||||
|
||||
#define B3_MPR_TOLERANCE 1E-6f
|
||||
#define B3_MPR_MAX_ITERATIONS 1000
|
||||
|
||||
struct _b3MprSupport_t
|
||||
{
|
||||
b3Float4 v; //!< Support point in minkowski sum
|
||||
b3Float4 v1; //!< Support point in obj1
|
||||
b3Float4 v2; //!< Support point in obj2
|
||||
};
|
||||
typedef struct _b3MprSupport_t b3MprSupport_t;
|
||||
|
||||
struct _b3MprSimplex_t
|
||||
{
|
||||
b3MprSupport_t ps[4];
|
||||
int last; //!< index of last added point
|
||||
};
|
||||
typedef struct _b3MprSimplex_t b3MprSimplex_t;
|
||||
|
||||
inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
|
||||
{
|
||||
return &s->ps[idx];
|
||||
}
|
||||
|
||||
inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
|
||||
{
|
||||
s->last = size - 1;
|
||||
}
|
||||
|
||||
|
||||
inline int b3MprSimplexSize(const b3MprSimplex_t *s)
|
||||
{
|
||||
return s->last + 1;
|
||||
}
|
||||
|
||||
|
||||
inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)
|
||||
{
|
||||
// here is no check on boundaries
|
||||
return &s->ps[idx];
|
||||
}
|
||||
|
||||
inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
|
||||
{
|
||||
*d = *s;
|
||||
}
|
||||
|
||||
inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
|
||||
{
|
||||
b3MprSupportCopy(s->ps + pos, a);
|
||||
}
|
||||
|
||||
|
||||
inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
|
||||
{
|
||||
b3MprSupport_t supp;
|
||||
|
||||
b3MprSupportCopy(&supp, &s->ps[pos1]);
|
||||
b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
|
||||
b3MprSupportCopy(&s->ps[pos2], &supp);
|
||||
}
|
||||
|
||||
|
||||
inline int b3MprIsZero(float val)
|
||||
{
|
||||
return B3_MPR_FABS(val) < FLT_EPSILON;
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline int b3MprEq(float _a, float _b)
|
||||
{
|
||||
float ab;
|
||||
float a, b;
|
||||
|
||||
ab = B3_MPR_FABS(_a - _b);
|
||||
if (B3_MPR_FABS(ab) < FLT_EPSILON)
|
||||
return 1;
|
||||
|
||||
a = B3_MPR_FABS(_a);
|
||||
b = B3_MPR_FABS(_b);
|
||||
if (b > a){
|
||||
return ab < FLT_EPSILON * b;
|
||||
}else{
|
||||
return ab < FLT_EPSILON * a;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)
|
||||
{
|
||||
return b3MprEq((*a).x, (*b).x)
|
||||
&& b3MprEq((*a).y, (*b).y)
|
||||
&& b3MprEq((*a).z, (*b).z);
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA)
|
||||
{
|
||||
b3Float4 supVec = b3MakeFloat4(0,0,0,0);
|
||||
float maxDot = -B3_LARGE_FLOAT;
|
||||
|
||||
if( 0 < hull->m_numVertices )
|
||||
{
|
||||
const b3Float4 scaled = supportVec;
|
||||
int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
|
||||
return verticesA[hull->m_vertexOffset+index];
|
||||
}
|
||||
|
||||
return supVec;
|
||||
|
||||
}
|
||||
|
||||
|
||||
B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4* sepAxis,
|
||||
const b3Float4* _dir, b3Float4* outp, int logme)
|
||||
{
|
||||
//dir is in worldspace, move to local space
|
||||
|
||||
b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
|
||||
b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
|
||||
|
||||
b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);
|
||||
|
||||
|
||||
//find local support vertex
|
||||
int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
|
||||
|
||||
b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);
|
||||
__global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
|
||||
|
||||
b3Float4 pInA;
|
||||
if (logme)
|
||||
{
|
||||
|
||||
|
||||
// b3Float4 supVec = b3MakeFloat4(0,0,0,0);
|
||||
float maxDot = -B3_LARGE_FLOAT;
|
||||
|
||||
if( 0 < hull->m_numVertices )
|
||||
{
|
||||
const b3Float4 scaled = localDir;
|
||||
int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
|
||||
pInA = cpuVertices[hull->m_vertexOffset+index];
|
||||
|
||||
}
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);
|
||||
}
|
||||
|
||||
//move vertex to world space
|
||||
*outp = b3TransformPoint(pInA,pos,orn);
|
||||
|
||||
}
|
||||
|
||||
inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4* sepAxis,
|
||||
const b3Float4* _dir, b3MprSupport_t *supp)
|
||||
{
|
||||
b3Float4 dir;
|
||||
dir = *_dir;
|
||||
b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);
|
||||
dir = *_dir*-1.f;
|
||||
b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);
|
||||
supp->v = supp->v1 - supp->v2;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
|
||||
{
|
||||
|
||||
center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
|
||||
center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
|
||||
center->v = center->v1 - center->v2;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
|
||||
{
|
||||
(*v).x = x;
|
||||
(*v).y = y;
|
||||
(*v).z = z;
|
||||
(*v).w = 0.f;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
|
||||
{
|
||||
(*v).x += (*w).x;
|
||||
(*v).y += (*w).y;
|
||||
(*v).z += (*w).z;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
|
||||
{
|
||||
*v = *w;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Scale(b3Float4 *d, float k)
|
||||
{
|
||||
*d *= k;
|
||||
}
|
||||
|
||||
inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
float dot;
|
||||
|
||||
dot = b3Dot3F4(*a,*b);
|
||||
return dot;
|
||||
}
|
||||
|
||||
|
||||
inline float b3MprVec3Len2(const b3Float4 *v)
|
||||
{
|
||||
return b3MprVec3Dot(v, v);
|
||||
}
|
||||
|
||||
inline void b3MprVec3Normalize(b3Float4 *d)
|
||||
{
|
||||
float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
|
||||
b3MprVec3Scale(d, k);
|
||||
}
|
||||
|
||||
inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
*d = b3Cross3(*a,*b);
|
||||
|
||||
}
|
||||
|
||||
|
||||
inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
|
||||
{
|
||||
*d = *v - *w;
|
||||
}
|
||||
|
||||
inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
|
||||
{
|
||||
b3Float4 v2v1, v3v1;
|
||||
|
||||
b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b3MprVec3Cross(dir, &v2v1, &v3v1);
|
||||
b3MprVec3Normalize(dir);
|
||||
}
|
||||
|
||||
|
||||
inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
|
||||
const b3Float4 *dir)
|
||||
{
|
||||
float dot;
|
||||
dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
|
||||
return b3MprIsZero(dot) || dot > 0.f;
|
||||
}
|
||||
|
||||
inline int portalReachTolerance(const b3MprSimplex_t *portal,
|
||||
const b3MprSupport_t *v4,
|
||||
const b3Float4 *dir)
|
||||
{
|
||||
float dv1, dv2, dv3, dv4;
|
||||
float dot1, dot2, dot3;
|
||||
|
||||
// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
|
||||
|
||||
dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
|
||||
dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
|
||||
dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
|
||||
dv4 = b3MprVec3Dot(&v4->v, dir);
|
||||
|
||||
dot1 = dv4 - dv1;
|
||||
dot2 = dv4 - dv2;
|
||||
dot3 = dv4 - dv3;
|
||||
|
||||
dot1 = B3_MPR_FMIN(dot1, dot2);
|
||||
dot1 = B3_MPR_FMIN(dot1, dot3);
|
||||
|
||||
return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
|
||||
}
|
||||
|
||||
inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,
|
||||
const b3MprSupport_t *v4,
|
||||
const b3Float4 *dir)
|
||||
{
|
||||
float dot;
|
||||
dot = b3MprVec3Dot(&v4->v, dir);
|
||||
return b3MprIsZero(dot) || dot > 0.f;
|
||||
}
|
||||
|
||||
inline void b3ExpandPortal(b3MprSimplex_t *portal,
|
||||
const b3MprSupport_t *v4)
|
||||
{
|
||||
float dot;
|
||||
b3Float4 v4v0;
|
||||
|
||||
b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
|
||||
if (dot > 0.f){
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
|
||||
if (dot > 0.f){
|
||||
b3MprSimplexSet(portal, 1, v4);
|
||||
}else{
|
||||
b3MprSimplexSet(portal, 3, v4);
|
||||
}
|
||||
}else{
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
|
||||
if (dot > 0.f){
|
||||
b3MprSimplexSet(portal, 2, v4);
|
||||
}else{
|
||||
b3MprSimplexSet(portal, 1, v4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4* sepAxis,
|
||||
__global int* hasSepAxis,
|
||||
b3MprSimplex_t *portal)
|
||||
{
|
||||
b3Float4 dir, va, vb;
|
||||
float dot;
|
||||
int cont;
|
||||
|
||||
|
||||
|
||||
// vertex 0 is center of portal
|
||||
b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));
|
||||
// vertex 0 is center of portal
|
||||
b3MprSimplexSetSize(portal, 1);
|
||||
|
||||
|
||||
|
||||
b3Float4 zero = b3MakeFloat4(0,0,0,0);
|
||||
b3Float4* b3mpr_vec3_origin = &zero;
|
||||
|
||||
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){
|
||||
// Portal's center lies on origin (0,0,0) => we know that objects
|
||||
// intersect but we would need to know penetration info.
|
||||
// So move center little bit...
|
||||
b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
|
||||
b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
|
||||
}
|
||||
|
||||
|
||||
// vertex 1 = support in direction of origin
|
||||
b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Scale(&dir, -1.f);
|
||||
b3MprVec3Normalize(&dir);
|
||||
|
||||
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));
|
||||
|
||||
b3MprSimplexSetSize(portal, 2);
|
||||
|
||||
// test if origin isn't outside of v1
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
|
||||
|
||||
|
||||
if (b3MprIsZero(dot) || dot < 0.f)
|
||||
return -1;
|
||||
|
||||
|
||||
// vertex 2
|
||||
b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
if (b3MprIsZero(b3MprVec3Len2(&dir))){
|
||||
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){
|
||||
// origin lies on v1
|
||||
return 1;
|
||||
}else{
|
||||
// origin lies on v0-v1 segment
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
|
||||
b3MprVec3Normalize(&dir);
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));
|
||||
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
|
||||
if (b3MprIsZero(dot) || dot < 0.f)
|
||||
return -1;
|
||||
|
||||
b3MprSimplexSetSize(portal, 3);
|
||||
|
||||
// vertex 3 direction
|
||||
b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Cross(&dir, &va, &vb);
|
||||
b3MprVec3Normalize(&dir);
|
||||
|
||||
// it is better to form portal faces to be oriented "outside" origin
|
||||
dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
|
||||
if (dot > 0.f){
|
||||
b3MprSimplexSwap(portal, 1, 2);
|
||||
b3MprVec3Scale(&dir, -1.f);
|
||||
}
|
||||
|
||||
while (b3MprSimplexSize(portal) < 4){
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));
|
||||
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
|
||||
if (b3MprIsZero(dot) || dot < 0.f)
|
||||
return -1;
|
||||
|
||||
cont = 0;
|
||||
|
||||
// test if origin is outside (v1, v0, v3) - set v2 as v3 and
|
||||
// continue
|
||||
b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 3)->v);
|
||||
dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
|
||||
if (dot < 0.f && !b3MprIsZero(dot)){
|
||||
b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
|
||||
cont = 1;
|
||||
}
|
||||
|
||||
if (!cont){
|
||||
// test if origin is outside (v3, v0, v2) - set v1 as v3 and
|
||||
// continue
|
||||
b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
|
||||
if (dot < 0.f && !b3MprIsZero(dot)){
|
||||
b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
|
||||
cont = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (cont){
|
||||
b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Cross(&dir, &va, &vb);
|
||||
b3MprVec3Normalize(&dir);
|
||||
}else{
|
||||
b3MprSimplexSetSize(portal, 4);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4* sepAxis,
|
||||
b3MprSimplex_t *portal)
|
||||
{
|
||||
b3Float4 dir;
|
||||
b3MprSupport_t v4;
|
||||
|
||||
for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)
|
||||
//while (1)
|
||||
{
|
||||
// compute direction outside the portal (from v0 throught v1,v2,v3
|
||||
// face)
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
// test if origin is inside the portal
|
||||
if (portalEncapsulesOrigin(portal, &dir))
|
||||
return 0;
|
||||
|
||||
// get next support point
|
||||
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
|
||||
|
||||
|
||||
// test if v4 can expand portal to contain origin and if portal
|
||||
// expanding doesn't reach given tolerance
|
||||
if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
|
||||
|| portalReachTolerance(portal, &v4, &dir))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// v1-v2-v3 triangle must be rearranged to face outside Minkowski
|
||||
// difference (direction from v0).
|
||||
b3ExpandPortal(portal, &v4);
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
|
||||
{
|
||||
|
||||
b3Float4 zero = b3MakeFloat4(0,0,0,0);
|
||||
b3Float4* b3mpr_vec3_origin = &zero;
|
||||
|
||||
b3Float4 dir;
|
||||
size_t i;
|
||||
float b[4], sum, inv;
|
||||
b3Float4 vec, p1, p2;
|
||||
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
// use barycentric coordinates of tetrahedron to find origin
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
|
||||
|
||||
sum = b[0] + b[1] + b[2] + b[3];
|
||||
|
||||
if (b3MprIsZero(sum) || sum < 0.f){
|
||||
b[0] = 0.f;
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 3)->v);
|
||||
b[1] = b3MprVec3Dot(&vec, &dir);
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b[2] = b3MprVec3Dot(&vec, &dir);
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
b[3] = b3MprVec3Dot(&vec, &dir);
|
||||
|
||||
sum = b[1] + b[2] + b[3];
|
||||
}
|
||||
|
||||
inv = 1.f / sum;
|
||||
|
||||
b3MprVec3Copy(&p1, b3mpr_vec3_origin);
|
||||
b3MprVec3Copy(&p2, b3mpr_vec3_origin);
|
||||
for (i = 0; i < 4; i++){
|
||||
b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
|
||||
b3MprVec3Scale(&vec, b[i]);
|
||||
b3MprVec3Add(&p1, &vec);
|
||||
|
||||
b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
|
||||
b3MprVec3Scale(&vec, b[i]);
|
||||
b3MprVec3Add(&p2, &vec);
|
||||
}
|
||||
b3MprVec3Scale(&p1, inv);
|
||||
b3MprVec3Scale(&p2, inv);
|
||||
|
||||
b3MprVec3Copy(pos, &p1);
|
||||
b3MprVec3Add(pos, &p2);
|
||||
b3MprVec3Scale(pos, 0.5);
|
||||
}
|
||||
|
||||
inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
b3Float4 ab;
|
||||
b3MprVec3Sub2(&ab, a, b);
|
||||
return b3MprVec3Len2(&ab);
|
||||
}
|
||||
|
||||
inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
|
||||
const b3Float4 *x0,
|
||||
const b3Float4 *b,
|
||||
b3Float4 *witness)
|
||||
{
|
||||
// The computation comes from solving equation of segment:
|
||||
// S(t) = x0 + t.d
|
||||
// where - x0 is initial point of segment
|
||||
// - d is direction of segment from x0 (|d| > 0)
|
||||
// - t belongs to <0, 1> interval
|
||||
//
|
||||
// Than, distance from a segment to some point P can be expressed:
|
||||
// D(t) = |x0 + t.d - P|^2
|
||||
// which is distance from any point on segment. Minimization
|
||||
// of this function brings distance from P to segment.
|
||||
// Minimization of D(t) leads to simple quadratic equation that's
|
||||
// solving is straightforward.
|
||||
//
|
||||
// Bonus of this method is witness point for free.
|
||||
|
||||
float dist, t;
|
||||
b3Float4 d, a;
|
||||
|
||||
// direction of segment
|
||||
b3MprVec3Sub2(&d, b, x0);
|
||||
|
||||
// precompute vector from P to x0
|
||||
b3MprVec3Sub2(&a, x0, P);
|
||||
|
||||
t = -1.f * b3MprVec3Dot(&a, &d);
|
||||
t /= b3MprVec3Len2(&d);
|
||||
|
||||
if (t < 0.f || b3MprIsZero(t)){
|
||||
dist = b3MprVec3Dist2(x0, P);
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, x0);
|
||||
}else if (t > 1.f || b3MprEq(t, 1.f)){
|
||||
dist = b3MprVec3Dist2(b, P);
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, b);
|
||||
}else{
|
||||
if (witness){
|
||||
b3MprVec3Copy(witness, &d);
|
||||
b3MprVec3Scale(witness, t);
|
||||
b3MprVec3Add(witness, x0);
|
||||
dist = b3MprVec3Dist2(witness, P);
|
||||
}else{
|
||||
// recycling variables
|
||||
b3MprVec3Scale(&d, t);
|
||||
b3MprVec3Add(&d, &a);
|
||||
dist = b3MprVec3Len2(&d);
|
||||
}
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
|
||||
inline float b3MprVec3PointTriDist2(const b3Float4 *P,
|
||||
const b3Float4 *x0, const b3Float4 *B,
|
||||
const b3Float4 *C,
|
||||
b3Float4 *witness)
|
||||
{
|
||||
// Computation comes from analytic expression for triangle (x0, B, C)
|
||||
// T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
|
||||
// Then equation for distance is:
|
||||
// D(s, t) = | T(s, t) - P |^2
|
||||
// This leads to minimization of quadratic function of two variables.
|
||||
// The solution from is taken only if s is between 0 and 1, t is
|
||||
// between 0 and 1 and t + s < 1, otherwise distance from segment is
|
||||
// computed.
|
||||
|
||||
b3Float4 d1, d2, a;
|
||||
float u, v, w, p, q, r;
|
||||
float s, t, dist, dist2;
|
||||
b3Float4 witness2;
|
||||
|
||||
b3MprVec3Sub2(&d1, B, x0);
|
||||
b3MprVec3Sub2(&d2, C, x0);
|
||||
b3MprVec3Sub2(&a, x0, P);
|
||||
|
||||
u = b3MprVec3Dot(&a, &a);
|
||||
v = b3MprVec3Dot(&d1, &d1);
|
||||
w = b3MprVec3Dot(&d2, &d2);
|
||||
p = b3MprVec3Dot(&a, &d1);
|
||||
q = b3MprVec3Dot(&a, &d2);
|
||||
r = b3MprVec3Dot(&d1, &d2);
|
||||
|
||||
s = (q * r - w * p) / (w * v - r * r);
|
||||
t = (-s * r - q) / w;
|
||||
|
||||
if ((b3MprIsZero(s) || s > 0.f)
|
||||
&& (b3MprEq(s, 1.f) || s < 1.f)
|
||||
&& (b3MprIsZero(t) || t > 0.f)
|
||||
&& (b3MprEq(t, 1.f) || t < 1.f)
|
||||
&& (b3MprEq(t + s, 1.f) || t + s < 1.f)){
|
||||
|
||||
if (witness){
|
||||
b3MprVec3Scale(&d1, s);
|
||||
b3MprVec3Scale(&d2, t);
|
||||
b3MprVec3Copy(witness, x0);
|
||||
b3MprVec3Add(witness, &d1);
|
||||
b3MprVec3Add(witness, &d2);
|
||||
|
||||
dist = b3MprVec3Dist2(witness, P);
|
||||
}else{
|
||||
dist = s * s * v;
|
||||
dist += t * t * w;
|
||||
dist += 2.f * s * t * r;
|
||||
dist += 2.f * s * p;
|
||||
dist += 2.f * t * q;
|
||||
dist += u;
|
||||
}
|
||||
}else{
|
||||
dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
|
||||
|
||||
dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
|
||||
if (dist2 < dist){
|
||||
dist = dist2;
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, &witness2);
|
||||
}
|
||||
|
||||
dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
|
||||
if (dist2 < dist){
|
||||
dist = dist2;
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, &witness2);
|
||||
}
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
|
||||
B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4* sepAxis,
|
||||
b3MprSimplex_t *portal,
|
||||
float *depth, b3Float4 *pdir, b3Float4 *pos)
|
||||
{
|
||||
b3Float4 dir;
|
||||
b3MprSupport_t v4;
|
||||
unsigned long iterations;
|
||||
|
||||
b3Float4 zero = b3MakeFloat4(0,0,0,0);
|
||||
b3Float4* b3mpr_vec3_origin = &zero;
|
||||
|
||||
|
||||
iterations = 1UL;
|
||||
for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)
|
||||
//while (1)
|
||||
{
|
||||
// compute portal direction and obtain next support point
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
|
||||
|
||||
|
||||
// reached tolerance -> find penetration info
|
||||
if (portalReachTolerance(portal, &v4, &dir)
|
||||
|| iterations ==B3_MPR_MAX_ITERATIONS)
|
||||
{
|
||||
*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);
|
||||
*depth = B3_MPR_SQRT(*depth);
|
||||
|
||||
if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
|
||||
{
|
||||
|
||||
*pdir = dir;
|
||||
}
|
||||
b3MprVec3Normalize(pdir);
|
||||
|
||||
// barycentric coordinates:
|
||||
b3FindPos(portal, pos);
|
||||
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
b3ExpandPortal(portal, &v4);
|
||||
|
||||
iterations++;
|
||||
}
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)
|
||||
{
|
||||
// Touching contact on portal's v1 - so depth is zero and direction
|
||||
// is unimportant and pos can be guessed
|
||||
*depth = 0.f;
|
||||
b3Float4 zero = b3MakeFloat4(0,0,0,0);
|
||||
b3Float4* b3mpr_vec3_origin = &zero;
|
||||
|
||||
|
||||
b3MprVec3Copy(dir, b3mpr_vec3_origin);
|
||||
|
||||
b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
|
||||
b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
|
||||
b3MprVec3Scale(pos, 0.5);
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
|
||||
float *depth, b3Float4 *dir, b3Float4 *pos)
|
||||
{
|
||||
|
||||
// Origin lies on v0-v1 segment.
|
||||
// Depth is distance to v1, direction also and position must be
|
||||
// computed
|
||||
|
||||
b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
|
||||
b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
|
||||
b3MprVec3Scale(pos, 0.5f);
|
||||
|
||||
|
||||
b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
|
||||
*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
|
||||
b3MprVec3Normalize(dir);
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
|
||||
b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4* sepAxis,
|
||||
__global int* hasSepAxis,
|
||||
float *depthOut, b3Float4* dirOut, b3Float4* posOut)
|
||||
{
|
||||
|
||||
b3MprSimplex_t portal;
|
||||
|
||||
|
||||
// if (!hasSepAxis[pairIndex])
|
||||
// return -1;
|
||||
|
||||
hasSepAxis[pairIndex] = 0;
|
||||
int res;
|
||||
|
||||
// Phase 1: Portal discovery
|
||||
res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);
|
||||
|
||||
|
||||
//sepAxis[pairIndex] = *pdir;//or -dir?
|
||||
|
||||
switch (res)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
// Phase 2: Portal refinement
|
||||
|
||||
res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);
|
||||
if (res < 0)
|
||||
return -1;
|
||||
|
||||
// Phase 3. Penetration info
|
||||
b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);
|
||||
hasSepAxis[pairIndex] = 1;
|
||||
sepAxis[pairIndex] = -*dirOut;
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
// Touching contact on portal's v1.
|
||||
b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
|
||||
b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
hasSepAxis[pairIndex]=0;
|
||||
//if (res < 0)
|
||||
//{
|
||||
// Origin isn't inside portal - no collision.
|
||||
return -1;
|
||||
//}
|
||||
}
|
||||
};
|
||||
|
||||
return 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //B3_MPR_PENETRATION_H
|
196
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h
vendored
Normal file
196
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h
vendored
Normal file
|
@ -0,0 +1,196 @@
|
|||
|
||||
#ifndef B3_NEW_CONTACT_REDUCTION_H
|
||||
#define B3_NEW_CONTACT_REDUCTION_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
|
||||
|
||||
|
||||
int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
|
||||
{
|
||||
if( nPoints == 0 )
|
||||
return 0;
|
||||
|
||||
if (nPoints <=4)
|
||||
return nPoints;
|
||||
|
||||
|
||||
if (nPoints >64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0,0,0,0);
|
||||
{
|
||||
|
||||
for (int i=0;i<nPoints;i++)
|
||||
center += p[i];
|
||||
center /= (float)nPoints;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// sample 4 directions
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross( nearNormal, aVector );
|
||||
b3Float4 v = b3Cross( nearNormal, u );
|
||||
u = b3Normalized( u );
|
||||
v = b3Normalized( v );
|
||||
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW= FLT_MAX;
|
||||
|
||||
int minIndex=-1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for(int ie = 0; ie<nPoints; ie++ )
|
||||
{
|
||||
if (p[ie].w<minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex=ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie]-center;
|
||||
f = b3Dot( u, r );
|
||||
if (f<maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot( -u, r );
|
||||
if (f<maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
|
||||
f = b3Dot( v, r );
|
||||
if (f<maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot( -v, r );
|
||||
if (f<maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
|
||||
}
|
||||
|
||||
__kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
|
||||
__global const b3RigidBodyData_t* rigidBodies,
|
||||
__global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global struct b3Contact4Data* globalContactsOut,
|
||||
__global b3Int4* clippingFaces,
|
||||
__global b3Float4* worldVertsB2,
|
||||
volatile __global int* nGlobalContactsOut,
|
||||
int vertexFaceCapacity,
|
||||
int contactCapacity,
|
||||
int numPairs,
|
||||
int pairIndex
|
||||
)
|
||||
{
|
||||
// int i = get_global_id(0);
|
||||
//int pairIndex = i;
|
||||
int i = pairIndex;
|
||||
|
||||
b3Int4 contactIdx;
|
||||
contactIdx=b3MakeInt4(0,1,2,3);
|
||||
|
||||
if (i<numPairs)
|
||||
{
|
||||
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
int nPoints = clippingFaces[pairIndex].w;
|
||||
|
||||
if (nPoints>0)
|
||||
{
|
||||
|
||||
__global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity];
|
||||
b3Float4 normal = -separatingNormals[i];
|
||||
|
||||
int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
|
||||
|
||||
int dstIdx;
|
||||
dstIdx = b3AtomicInc( nGlobalContactsOut);
|
||||
|
||||
//#if 0
|
||||
b3Assert(dstIdx < contactCapacity);
|
||||
if (dstIdx < contactCapacity)
|
||||
{
|
||||
|
||||
__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = -normal;
|
||||
c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
|
||||
c->m_batchIdx = pairIndex;
|
||||
int bodyA = pairs[pairIndex].x;
|
||||
int bodyB = pairs[pairIndex].y;
|
||||
|
||||
pairs[pairIndex].w = dstIdx;
|
||||
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
|
||||
c->m_childIndexA =-1;
|
||||
c->m_childIndexB =-1;
|
||||
|
||||
switch (nReducedContacts)
|
||||
{
|
||||
case 4:
|
||||
c->m_worldPosB[3] = pointsIn[contactIdx.w];
|
||||
case 3:
|
||||
c->m_worldPosB[2] = pointsIn[contactIdx.z];
|
||||
case 2:
|
||||
c->m_worldPosB[1] = pointsIn[contactIdx.y];
|
||||
case 1:
|
||||
c->m_worldPosB[0] = pointsIn[contactIdx.x];
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
GET_NPOINTS(*c) = nReducedContacts;
|
||||
|
||||
}
|
||||
|
||||
|
||||
//#endif
|
||||
|
||||
}// if (numContactsOut>0)
|
||||
}// if (hasSeparatingAxis[i])
|
||||
}// if (i<numPairs)
|
||||
|
||||
|
||||
|
||||
}
|
||||
#endif
|
90
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
vendored
Normal file
90
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
vendored
Normal file
|
@ -0,0 +1,90 @@
|
|||
|
||||
|
||||
#ifndef B3_QUANTIZED_BVH_NODE_H
|
||||
#define B3_QUANTIZED_BVH_NODE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
#define B3_MAX_NUM_PARTS_IN_BITS 10
|
||||
|
||||
///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
|
||||
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
|
||||
typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
|
||||
|
||||
struct b3QuantizedBvhNodeData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
};
|
||||
|
||||
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
unsigned int x=0;
|
||||
unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
|
||||
// Get only the lower bits where the triangle index is stored
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
|
||||
}
|
||||
|
||||
inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
|
||||
}
|
||||
|
||||
inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
return -rootNode->m_escapeIndexOrTriangleIndex;
|
||||
}
|
||||
|
||||
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
|
||||
{
|
||||
b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
|
||||
clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
|
||||
|
||||
b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
|
||||
if (isMax)
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
|
||||
} else
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
|
||||
const unsigned short int* aabbMin1,
|
||||
const unsigned short int* aabbMax1,
|
||||
const unsigned short int* aabbMin2,
|
||||
const unsigned short int* aabbMax2)
|
||||
{
|
||||
//int overlap = 1;
|
||||
if (aabbMin1[0] > aabbMax2[0])
|
||||
return 0;
|
||||
if (aabbMax1[0] < aabbMin2[0])
|
||||
return 0;
|
||||
if (aabbMin1[1] > aabbMax2[1])
|
||||
return 0;
|
||||
if (aabbMax1[1] < aabbMin2[1])
|
||||
return 0;
|
||||
if (aabbMin1[2] > aabbMax2[2])
|
||||
return 0;
|
||||
if (aabbMax1[2] < aabbMin2[2])
|
||||
return 0;
|
||||
return 1;
|
||||
//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
|
||||
//return overlap;
|
||||
}
|
||||
|
||||
|
||||
#endif //B3_QUANTIZED_BVH_NODE_H
|
97
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
vendored
Normal file
97
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h
vendored
Normal file
|
@ -0,0 +1,97 @@
|
|||
#ifndef B3_REDUCE_CONTACTS_H
|
||||
#define B3_REDUCE_CONTACTS_H
|
||||
|
||||
inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
|
||||
{
|
||||
if( nPoints == 0 )
|
||||
return 0;
|
||||
|
||||
if (nPoints <=4)
|
||||
return nPoints;
|
||||
|
||||
|
||||
if (nPoints >64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0,0,0,0);
|
||||
{
|
||||
|
||||
for (int i=0;i<nPoints;i++)
|
||||
center += p[i];
|
||||
center /= (float)nPoints;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// sample 4 directions
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross3( nearNormal, aVector );
|
||||
b3Float4 v = b3Cross3( nearNormal, u );
|
||||
u = b3FastNormalized3( u );
|
||||
v = b3FastNormalized3( v );
|
||||
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW= FLT_MAX;
|
||||
|
||||
int minIndex=-1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for(int ie = 0; ie<nPoints; ie++ )
|
||||
{
|
||||
if (p[ie].w<minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex=ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie]-center;
|
||||
f = b3Dot3F4( u, r );
|
||||
if (f<maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4( -u, r );
|
||||
if (f<maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
|
||||
f = b3Dot3F4( v, r );
|
||||
if (f<maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4( -v, r );
|
||||
if (f<maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
|
||||
}
|
||||
|
||||
#endif //B3_REDUCE_CONTACTS_H
|
34
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h
vendored
Normal file
34
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h
vendored
Normal file
|
@ -0,0 +1,34 @@
|
|||
#ifndef B3_RIGIDBODY_DATA_H
|
||||
#define B3_RIGIDBODY_DATA_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
#include "Bullet3Common/shared/b3Mat3x3.h"
|
||||
|
||||
typedef struct b3RigidBodyData b3RigidBodyData_t;
|
||||
|
||||
|
||||
struct b3RigidBodyData
|
||||
{
|
||||
b3Float4 m_pos;
|
||||
b3Quat m_quat;
|
||||
b3Float4 m_linVel;
|
||||
b3Float4 m_angVel;
|
||||
|
||||
int m_collidableIdx;
|
||||
float m_invMass;
|
||||
float m_restituitionCoeff;
|
||||
float m_frictionCoeff;
|
||||
};
|
||||
|
||||
typedef struct b3InertiaData b3InertiaData_t;
|
||||
|
||||
struct b3InertiaData
|
||||
{
|
||||
b3Mat3x3 m_invInertiaWorld;
|
||||
b3Mat3x3 m_initInvInertia;
|
||||
};
|
||||
|
||||
|
||||
#endif //B3_RIGIDBODY_DATA_H
|
||||
|
40
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h
vendored
Normal file
40
thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h
vendored
Normal file
|
@ -0,0 +1,40 @@
|
|||
#ifndef B3_UPDATE_AABBS_H
|
||||
#define B3_UPDATE_AABBS_H
|
||||
|
||||
|
||||
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
|
||||
|
||||
void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
|
||||
{
|
||||
__global const b3RigidBodyData_t* body = &bodies[bodyId];
|
||||
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
|
||||
if (shapeIndex>=0)
|
||||
{
|
||||
|
||||
b3Aabb_t localAabb = localShapeAABB[collidableIndex];
|
||||
b3Aabb_t worldAabb;
|
||||
|
||||
b3Float4 aabbAMinOut,aabbAMaxOut;
|
||||
float margin = 0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);
|
||||
|
||||
worldAabb.m_minVec =aabbAMinOut;
|
||||
worldAabb.m_minIndices[3] = bodyId;
|
||||
worldAabb.m_maxVec = aabbAMaxOut;
|
||||
worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;
|
||||
worldAabbs[bodyId] = worldAabb;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_UPDATE_AABBS_H
|
13
thirdparty/bullet/src/Bullet3Collision/premake4.lua
vendored
Normal file
13
thirdparty/bullet/src/Bullet3Collision/premake4.lua
vendored
Normal file
|
@ -0,0 +1,13 @@
|
|||
project "Bullet3Collision"
|
||||
|
||||
language "C++"
|
||||
|
||||
kind "StaticLib"
|
||||
|
||||
includedirs {".."}
|
||||
|
||||
|
||||
files {
|
||||
"**.cpp",
|
||||
"**.h"
|
||||
}
|
63
thirdparty/bullet/src/Bullet3Common/CMakeLists.txt
vendored
Normal file
63
thirdparty/bullet/src/Bullet3Common/CMakeLists.txt
vendored
Normal file
|
@ -0,0 +1,63 @@
|
|||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
)
|
||||
|
||||
SET(Bullet3Common_SRCS
|
||||
b3AlignedAllocator.cpp
|
||||
b3Vector3.cpp
|
||||
b3Logging.cpp
|
||||
)
|
||||
|
||||
SET(Bullet3Common_HDRS
|
||||
b3AlignedAllocator.h
|
||||
b3AlignedObjectArray.h
|
||||
b3CommandLineArgs.h
|
||||
b3HashMap.h
|
||||
b3Logging.h
|
||||
b3Matrix3x3.h
|
||||
b3MinMax.h
|
||||
b3PoolAllocator.h
|
||||
b3QuadWord.h
|
||||
b3Quaternion.h
|
||||
b3Random.h
|
||||
b3Scalar.h
|
||||
b3StackAlloc.h
|
||||
b3Transform.h
|
||||
b3TransformUtil.h
|
||||
b3Vector3.h
|
||||
shared/b3Float4
|
||||
shared/b3Int2.h
|
||||
shared/b3Int4.h
|
||||
shared/b3Mat3x3.h
|
||||
shared/b3PlatformDefinitions
|
||||
shared/b3Quat.h
|
||||
)
|
||||
|
||||
ADD_LIBRARY(Bullet3Common ${Bullet3Common_SRCS} ${Bullet3Common_HDRS})
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#FILES_MATCHING requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Common DESTINATION .)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Common
|
||||
RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
|
||||
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES FRAMEWORK true)
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES PUBLIC_HEADER "${Bullet3Common_HDRS}")
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
181
thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.cpp
vendored
Normal file
181
thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.cpp
vendored
Normal file
|
@ -0,0 +1,181 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
int b3g_numAlignedAllocs = 0;
|
||||
int b3g_numAlignedFree = 0;
|
||||
int b3g_totalBytesAlignedAllocs = 0;//detect memory leaks
|
||||
|
||||
static void *b3AllocDefault(size_t size)
|
||||
{
|
||||
return malloc(size);
|
||||
}
|
||||
|
||||
static void b3FreeDefault(void *ptr)
|
||||
{
|
||||
free(ptr);
|
||||
}
|
||||
|
||||
static b3AllocFunc* b3s_allocFunc = b3AllocDefault;
|
||||
static b3FreeFunc* b3s_freeFunc = b3FreeDefault;
|
||||
|
||||
|
||||
|
||||
#if defined (B3_HAS_ALIGNED_ALLOCATOR)
|
||||
#include <malloc.h>
|
||||
static void *b3AlignedAllocDefault(size_t size, int alignment)
|
||||
{
|
||||
return _aligned_malloc(size, (size_t)alignment);
|
||||
}
|
||||
|
||||
static void b3AlignedFreeDefault(void *ptr)
|
||||
{
|
||||
_aligned_free(ptr);
|
||||
}
|
||||
#elif defined(__CELLOS_LV2__)
|
||||
#include <stdlib.h>
|
||||
|
||||
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
|
||||
{
|
||||
return memalign(alignment, size);
|
||||
}
|
||||
|
||||
static inline void b3AlignedFreeDefault(void *ptr)
|
||||
{
|
||||
free(ptr);
|
||||
}
|
||||
#else
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
|
||||
{
|
||||
void *ret;
|
||||
char *real;
|
||||
real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment-1));
|
||||
if (real) {
|
||||
ret = b3AlignPointer(real + sizeof(void *),alignment);
|
||||
*((void **)(ret)-1) = (void *)(real);
|
||||
} else {
|
||||
ret = (void *)(real);
|
||||
}
|
||||
return (ret);
|
||||
}
|
||||
|
||||
static inline void b3AlignedFreeDefault(void *ptr)
|
||||
{
|
||||
void* real;
|
||||
|
||||
if (ptr) {
|
||||
real = *((void **)(ptr)-1);
|
||||
b3s_freeFunc(real);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
static b3AlignedAllocFunc* b3s_alignedAllocFunc = b3AlignedAllocDefault;
|
||||
static b3AlignedFreeFunc* b3s_alignedFreeFunc = b3AlignedFreeDefault;
|
||||
|
||||
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
|
||||
{
|
||||
b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
|
||||
b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
|
||||
}
|
||||
|
||||
void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
|
||||
{
|
||||
b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
|
||||
b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
|
||||
}
|
||||
|
||||
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
//this generic allocator provides the total allocated number of bytes
|
||||
#include <stdio.h>
|
||||
|
||||
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename)
|
||||
{
|
||||
void *ret;
|
||||
char *real;
|
||||
|
||||
b3g_totalBytesAlignedAllocs += size;
|
||||
b3g_numAlignedAllocs++;
|
||||
|
||||
|
||||
real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1));
|
||||
if (real) {
|
||||
ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment);
|
||||
*((void **)(ret)-1) = (void *)(real);
|
||||
*((int*)(ret)-2) = size;
|
||||
|
||||
} else {
|
||||
ret = (void *)(real);//??
|
||||
}
|
||||
|
||||
b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size);
|
||||
|
||||
int* ptr = (int*)ret;
|
||||
*ptr = 12;
|
||||
return (ret);
|
||||
}
|
||||
|
||||
void b3AlignedFreeInternal (void* ptr,int line,char* filename)
|
||||
{
|
||||
|
||||
void* real;
|
||||
b3g_numAlignedFree++;
|
||||
|
||||
if (ptr) {
|
||||
real = *((void **)(ptr)-1);
|
||||
int size = *((int*)(ptr)-2);
|
||||
b3g_totalBytesAlignedAllocs -= size;
|
||||
|
||||
b3Printf("free #%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedFree,real, filename,line,size);
|
||||
|
||||
b3s_freeFunc(real);
|
||||
} else
|
||||
{
|
||||
b3Printf("NULL ptr\n");
|
||||
}
|
||||
}
|
||||
|
||||
#else //B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
|
||||
void* b3AlignedAllocInternal (size_t size, int alignment)
|
||||
{
|
||||
b3g_numAlignedAllocs++;
|
||||
void* ptr;
|
||||
ptr = b3s_alignedAllocFunc(size, alignment);
|
||||
// b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
void b3AlignedFreeInternal (void* ptr)
|
||||
{
|
||||
if (!ptr)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
b3g_numAlignedFree++;
|
||||
// b3Printf("b3AlignedFreeInternal %x\n",ptr);
|
||||
b3s_alignedFreeFunc(ptr);
|
||||
}
|
||||
|
||||
#endif //B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
|
107
thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.h
vendored
Normal file
107
thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.h
vendored
Normal file
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_ALIGNED_ALLOCATOR
|
||||
#define B3_ALIGNED_ALLOCATOR
|
||||
|
||||
///we probably replace this with our own aligned memory allocator
|
||||
///so we replace _aligned_malloc and _aligned_free with our own
|
||||
///that is better portable and more predictable
|
||||
|
||||
#include "b3Scalar.h"
|
||||
//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
|
||||
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
|
||||
#define b3AlignedAlloc(a,b) \
|
||||
b3AlignedAllocInternal(a,b,__LINE__,__FILE__)
|
||||
|
||||
#define b3AlignedFree(ptr) \
|
||||
b3AlignedFreeInternal(ptr,__LINE__,__FILE__)
|
||||
|
||||
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename);
|
||||
|
||||
void b3AlignedFreeInternal (void* ptr,int line,char* filename);
|
||||
|
||||
#else
|
||||
void* b3AlignedAllocInternal (size_t size, int alignment);
|
||||
void b3AlignedFreeInternal (void* ptr);
|
||||
|
||||
#define b3AlignedAlloc(size,alignment) b3AlignedAllocInternal(size,alignment)
|
||||
#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
|
||||
|
||||
#endif
|
||||
typedef int btSizeType;
|
||||
|
||||
typedef void *(b3AlignedAllocFunc)(size_t size, int alignment);
|
||||
typedef void (b3AlignedFreeFunc)(void *memblock);
|
||||
typedef void *(b3AllocFunc)(size_t size);
|
||||
typedef void (b3FreeFunc)(void *memblock);
|
||||
|
||||
///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
|
||||
void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc);
|
||||
///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
|
||||
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc);
|
||||
|
||||
|
||||
///The b3AlignedAllocator is a portable class for aligned memory allocations.
|
||||
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
|
||||
template < typename T , unsigned Alignment >
|
||||
class b3AlignedAllocator {
|
||||
|
||||
typedef b3AlignedAllocator< T , Alignment > self_type;
|
||||
|
||||
public:
|
||||
|
||||
//just going down a list:
|
||||
b3AlignedAllocator() {}
|
||||
/*
|
||||
b3AlignedAllocator( const self_type & ) {}
|
||||
*/
|
||||
|
||||
template < typename Other >
|
||||
b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {}
|
||||
|
||||
typedef const T* const_pointer;
|
||||
typedef const T& const_reference;
|
||||
typedef T* pointer;
|
||||
typedef T& reference;
|
||||
typedef T value_type;
|
||||
|
||||
pointer address ( reference ref ) const { return &ref; }
|
||||
const_pointer address ( const_reference ref ) const { return &ref; }
|
||||
pointer allocate ( btSizeType n , const_pointer * hint = 0 ) {
|
||||
(void)hint;
|
||||
return reinterpret_cast< pointer >(b3AlignedAlloc( sizeof(value_type) * n , Alignment ));
|
||||
}
|
||||
void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); }
|
||||
void deallocate( pointer ptr ) {
|
||||
b3AlignedFree( reinterpret_cast< void * >( ptr ) );
|
||||
}
|
||||
void destroy ( pointer ptr ) { ptr->~value_type(); }
|
||||
|
||||
|
||||
template < typename O > struct rebind {
|
||||
typedef b3AlignedAllocator< O , Alignment > other;
|
||||
};
|
||||
template < typename O >
|
||||
self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; }
|
||||
|
||||
friend bool operator==( const self_type & , const self_type & ) { return true; }
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //B3_ALIGNED_ALLOCATOR
|
||||
|
533
thirdparty/bullet/src/Bullet3Common/b3AlignedObjectArray.h
vendored
Normal file
533
thirdparty/bullet/src/Bullet3Common/b3AlignedObjectArray.h
vendored
Normal file
|
@ -0,0 +1,533 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef B3_OBJECT_ARRAY__
|
||||
#define B3_OBJECT_ARRAY__
|
||||
|
||||
#include "b3Scalar.h" // has definitions like B3_FORCE_INLINE
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
|
||||
///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
|
||||
///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
|
||||
///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
|
||||
///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
|
||||
|
||||
#define B3_USE_PLACEMENT_NEW 1
|
||||
//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
|
||||
#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
|
||||
|
||||
#ifdef B3_USE_MEMCPY
|
||||
#include <memory.h>
|
||||
#include <string.h>
|
||||
#endif //B3_USE_MEMCPY
|
||||
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
#include <new> //for placement new
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
|
||||
|
||||
///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
|
||||
///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
|
||||
template <typename T>
|
||||
//template <class T>
|
||||
class b3AlignedObjectArray
|
||||
{
|
||||
b3AlignedAllocator<T , 16> m_allocator;
|
||||
|
||||
int m_size;
|
||||
int m_capacity;
|
||||
T* m_data;
|
||||
//PCK: added this line
|
||||
bool m_ownsMemory;
|
||||
|
||||
#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
|
||||
public:
|
||||
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other)
|
||||
{
|
||||
copyFromArray(other);
|
||||
return *this;
|
||||
}
|
||||
#else//B3_ALLOW_ARRAY_COPY_OPERATOR
|
||||
private:
|
||||
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other);
|
||||
#endif//B3_ALLOW_ARRAY_COPY_OPERATOR
|
||||
|
||||
protected:
|
||||
B3_FORCE_INLINE int allocSize(int size)
|
||||
{
|
||||
return (size ? size*2 : 1);
|
||||
}
|
||||
B3_FORCE_INLINE void copy(int start,int end, T* dest) const
|
||||
{
|
||||
int i;
|
||||
for (i=start;i<end;++i)
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
new (&dest[i]) T(m_data[i]);
|
||||
#else
|
||||
dest[i] = m_data[i];
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void init()
|
||||
{
|
||||
//PCK: added this line
|
||||
m_ownsMemory = true;
|
||||
m_data = 0;
|
||||
m_size = 0;
|
||||
m_capacity = 0;
|
||||
}
|
||||
B3_FORCE_INLINE void destroy(int first,int last)
|
||||
{
|
||||
int i;
|
||||
for (i=first; i<last;i++)
|
||||
{
|
||||
m_data[i].~T();
|
||||
}
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void* allocate(int size)
|
||||
{
|
||||
if (size)
|
||||
return m_allocator.allocate(size);
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void deallocate()
|
||||
{
|
||||
if(m_data) {
|
||||
//PCK: enclosed the deallocation in this block
|
||||
if (m_ownsMemory)
|
||||
{
|
||||
m_allocator.deallocate(m_data);
|
||||
}
|
||||
m_data = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
b3AlignedObjectArray()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
~b3AlignedObjectArray()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
|
||||
b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
|
||||
{
|
||||
init();
|
||||
|
||||
int otherSize = otherArray.size();
|
||||
resize (otherSize);
|
||||
otherArray.copy(0, otherSize, m_data);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// return the number of elements in the array
|
||||
B3_FORCE_INLINE int size() const
|
||||
{
|
||||
return m_size;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const T& at(int n) const
|
||||
{
|
||||
b3Assert(n>=0);
|
||||
b3Assert(n<size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE T& at(int n)
|
||||
{
|
||||
b3Assert(n>=0);
|
||||
b3Assert(n<size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const T& operator[](int n) const
|
||||
{
|
||||
b3Assert(n>=0);
|
||||
b3Assert(n<size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE T& operator[](int n)
|
||||
{
|
||||
b3Assert(n>=0);
|
||||
b3Assert(n<size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
|
||||
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
|
||||
B3_FORCE_INLINE void clear()
|
||||
{
|
||||
destroy(0,size());
|
||||
|
||||
deallocate();
|
||||
|
||||
init();
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void pop_back()
|
||||
{
|
||||
b3Assert(m_size>0);
|
||||
m_size--;
|
||||
m_data[m_size].~T();
|
||||
}
|
||||
|
||||
|
||||
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
|
||||
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
|
||||
B3_FORCE_INLINE void resizeNoInitialize(int newsize)
|
||||
{
|
||||
int curSize = size();
|
||||
|
||||
if (newsize < curSize)
|
||||
{
|
||||
} else
|
||||
{
|
||||
if (newsize > size())
|
||||
{
|
||||
reserve(newsize);
|
||||
}
|
||||
//leave this uninitialized
|
||||
}
|
||||
m_size = newsize;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void resize(int newsize, const T& fillData=T())
|
||||
{
|
||||
int curSize = size();
|
||||
|
||||
if (newsize < curSize)
|
||||
{
|
||||
for(int i = newsize; i < curSize; i++)
|
||||
{
|
||||
m_data[i].~T();
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (newsize > size())
|
||||
{
|
||||
reserve(newsize);
|
||||
}
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
for (int i=curSize;i<newsize;i++)
|
||||
{
|
||||
new ( &m_data[i]) T(fillData);
|
||||
}
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
|
||||
}
|
||||
|
||||
m_size = newsize;
|
||||
}
|
||||
B3_FORCE_INLINE T& expandNonInitializing( )
|
||||
{
|
||||
int sz = size();
|
||||
if( sz == capacity() )
|
||||
{
|
||||
reserve( allocSize(size()) );
|
||||
}
|
||||
m_size++;
|
||||
|
||||
return m_data[sz];
|
||||
}
|
||||
|
||||
|
||||
B3_FORCE_INLINE T& expand( const T& fillValue=T())
|
||||
{
|
||||
int sz = size();
|
||||
if( sz == capacity() )
|
||||
{
|
||||
reserve( allocSize(size()) );
|
||||
}
|
||||
m_size++;
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
|
||||
#endif
|
||||
|
||||
return m_data[sz];
|
||||
}
|
||||
|
||||
|
||||
B3_FORCE_INLINE void push_back(const T& _Val)
|
||||
{
|
||||
int sz = size();
|
||||
if( sz == capacity() )
|
||||
{
|
||||
reserve( allocSize(size()) );
|
||||
}
|
||||
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
new ( &m_data[m_size] ) T(_Val);
|
||||
#else
|
||||
m_data[size()] = _Val;
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
|
||||
m_size++;
|
||||
}
|
||||
|
||||
|
||||
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
|
||||
B3_FORCE_INLINE int capacity() const
|
||||
{
|
||||
return m_capacity;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void reserve(int _Count)
|
||||
{ // determine new minimum length of allocated storage
|
||||
if (capacity() < _Count)
|
||||
{ // not enough room, reallocate
|
||||
T* s = (T*)allocate(_Count);
|
||||
b3Assert(s);
|
||||
if (s==0)
|
||||
{
|
||||
b3Error("b3AlignedObjectArray reserve out-of-memory\n");
|
||||
_Count=0;
|
||||
m_size=0;
|
||||
}
|
||||
copy(0, size(), s);
|
||||
|
||||
destroy(0,size());
|
||||
|
||||
deallocate();
|
||||
|
||||
//PCK: added this line
|
||||
m_ownsMemory = true;
|
||||
|
||||
m_data = s;
|
||||
|
||||
m_capacity = _Count;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
class less
|
||||
{
|
||||
public:
|
||||
|
||||
bool operator() ( const T& a, const T& b )
|
||||
{
|
||||
return ( a < b );
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
template <typename L>
|
||||
void quickSortInternal(const L& CompareFunc,int lo, int hi)
|
||||
{
|
||||
// lo is the lower index, hi is the upper index
|
||||
// of the region of array a that is to be sorted
|
||||
int i=lo, j=hi;
|
||||
T x=m_data[(lo+hi)/2];
|
||||
|
||||
// partition
|
||||
do
|
||||
{
|
||||
while (CompareFunc(m_data[i],x))
|
||||
i++;
|
||||
while (CompareFunc(x,m_data[j]))
|
||||
j--;
|
||||
if (i<=j)
|
||||
{
|
||||
swap(i,j);
|
||||
i++; j--;
|
||||
}
|
||||
} while (i<=j);
|
||||
|
||||
// recursion
|
||||
if (lo<j)
|
||||
quickSortInternal( CompareFunc, lo, j);
|
||||
if (i<hi)
|
||||
quickSortInternal( CompareFunc, i, hi);
|
||||
}
|
||||
|
||||
|
||||
template <typename L>
|
||||
void quickSort(const L& CompareFunc)
|
||||
{
|
||||
//don't sort 0 or 1 elements
|
||||
if (size()>1)
|
||||
{
|
||||
quickSortInternal(CompareFunc,0,size()-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
|
||||
template <typename L>
|
||||
void downHeap(T *pArr, int k, int n, const L& CompareFunc)
|
||||
{
|
||||
/* PRE: a[k+1..N] is a heap */
|
||||
/* POST: a[k..N] is a heap */
|
||||
|
||||
T temp = pArr[k - 1];
|
||||
/* k has child(s) */
|
||||
while (k <= n/2)
|
||||
{
|
||||
int child = 2*k;
|
||||
|
||||
if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
|
||||
{
|
||||
child++;
|
||||
}
|
||||
/* pick larger child */
|
||||
if (CompareFunc(temp , pArr[child - 1]))
|
||||
{
|
||||
/* move child up */
|
||||
pArr[k - 1] = pArr[child - 1];
|
||||
k = child;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
pArr[k - 1] = temp;
|
||||
} /*downHeap*/
|
||||
|
||||
void swap(int index0,int index1)
|
||||
{
|
||||
#ifdef B3_USE_MEMCPY
|
||||
char temp[sizeof(T)];
|
||||
memcpy(temp,&m_data[index0],sizeof(T));
|
||||
memcpy(&m_data[index0],&m_data[index1],sizeof(T));
|
||||
memcpy(&m_data[index1],temp,sizeof(T));
|
||||
#else
|
||||
T temp = m_data[index0];
|
||||
m_data[index0] = m_data[index1];
|
||||
m_data[index1] = temp;
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
|
||||
}
|
||||
|
||||
template <typename L>
|
||||
void heapSort(const L& CompareFunc)
|
||||
{
|
||||
/* sort a[0..N-1], N.B. 0 to N-1 */
|
||||
int k;
|
||||
int n = m_size;
|
||||
for (k = n/2; k > 0; k--)
|
||||
{
|
||||
downHeap(m_data, k, n, CompareFunc);
|
||||
}
|
||||
|
||||
/* a[1..N] is now a heap */
|
||||
while ( n>=1 )
|
||||
{
|
||||
swap(0,n-1); /* largest of a[0..n-1] */
|
||||
|
||||
|
||||
n = n - 1;
|
||||
/* restore a[1..i-1] heap */
|
||||
downHeap(m_data, 1, n, CompareFunc);
|
||||
}
|
||||
}
|
||||
|
||||
///non-recursive binary search, assumes sorted array
|
||||
int findBinarySearch(const T& key) const
|
||||
{
|
||||
int first = 0;
|
||||
int last = size()-1;
|
||||
|
||||
//assume sorted array
|
||||
while (first <= last) {
|
||||
int mid = (first + last) / 2; // compute mid point.
|
||||
if (key > m_data[mid])
|
||||
first = mid + 1; // repeat search in top half.
|
||||
else if (key < m_data[mid])
|
||||
last = mid - 1; // repeat search in bottom half.
|
||||
else
|
||||
return mid; // found it. return position /////
|
||||
}
|
||||
return size(); // failed to find key
|
||||
}
|
||||
|
||||
|
||||
int findLinearSearch(const T& key) const
|
||||
{
|
||||
int index=size();
|
||||
int i;
|
||||
|
||||
for (i=0;i<size();i++)
|
||||
{
|
||||
if (m_data[i] == key)
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
int findLinearSearch2(const T& key) const
|
||||
{
|
||||
int index=-1;
|
||||
int i;
|
||||
|
||||
for (i=0;i<size();i++)
|
||||
{
|
||||
if (m_data[i] == key)
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
void remove(const T& key)
|
||||
{
|
||||
|
||||
int findIndex = findLinearSearch(key);
|
||||
if (findIndex<size())
|
||||
{
|
||||
swap( findIndex,size()-1);
|
||||
pop_back();
|
||||
}
|
||||
}
|
||||
|
||||
//PCK: whole function
|
||||
void initializeFromBuffer(void *buffer, int size, int capacity)
|
||||
{
|
||||
clear();
|
||||
m_ownsMemory = false;
|
||||
m_data = (T*)buffer;
|
||||
m_size = size;
|
||||
m_capacity = capacity;
|
||||
}
|
||||
|
||||
void copyFromArray(const b3AlignedObjectArray& otherArray)
|
||||
{
|
||||
int otherSize = otherArray.size();
|
||||
resize (otherSize);
|
||||
otherArray.copy(0, otherSize, m_data);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //B3_OBJECT_ARRAY__
|
101
thirdparty/bullet/src/Bullet3Common/b3CommandLineArgs.h
vendored
Normal file
101
thirdparty/bullet/src/Bullet3Common/b3CommandLineArgs.h
vendored
Normal file
|
@ -0,0 +1,101 @@
|
|||
#ifndef COMMAND_LINE_ARGS_H
|
||||
#define COMMAND_LINE_ARGS_H
|
||||
|
||||
/******************************************************************************
|
||||
* Command-line parsing
|
||||
******************************************************************************/
|
||||
#include <map>
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <cstring>
|
||||
#include <sstream>
|
||||
class b3CommandLineArgs
|
||||
{
|
||||
protected:
|
||||
|
||||
std::map<std::string, std::string> pairs;
|
||||
|
||||
public:
|
||||
|
||||
// Constructor
|
||||
b3CommandLineArgs(int argc, char **argv)
|
||||
{
|
||||
addArgs(argc,argv);
|
||||
}
|
||||
|
||||
void addArgs(int argc, char**argv)
|
||||
{
|
||||
for (int i = 1; i < argc; i++)
|
||||
{
|
||||
std::string arg = argv[i];
|
||||
|
||||
if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-')) {
|
||||
continue;
|
||||
}
|
||||
|
||||
std::string::size_type pos;
|
||||
std::string key, val;
|
||||
if ((pos = arg.find( '=')) == std::string::npos) {
|
||||
key = std::string(arg, 2, arg.length() - 2);
|
||||
val = "";
|
||||
} else {
|
||||
key = std::string(arg, 2, pos - 2);
|
||||
val = std::string(arg, pos + 1, arg.length() - 1);
|
||||
}
|
||||
|
||||
//only add new keys, don't replace existing
|
||||
if(pairs.find(key) == pairs.end())
|
||||
{
|
||||
pairs[key] = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool CheckCmdLineFlag(const char* arg_name)
|
||||
{
|
||||
std::map<std::string, std::string>::iterator itr;
|
||||
if ((itr = pairs.find(arg_name)) != pairs.end()) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool GetCmdLineArgument(const char *arg_name, T &val);
|
||||
|
||||
int ParsedArgc()
|
||||
{
|
||||
return pairs.size();
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
|
||||
{
|
||||
std::map<std::string, std::string>::iterator itr;
|
||||
if ((itr = pairs.find(arg_name)) != pairs.end()) {
|
||||
std::istringstream strstream(itr->second);
|
||||
strstream >> val;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline bool b3CommandLineArgs::GetCmdLineArgument<char*>(const char* arg_name, char* &val)
|
||||
{
|
||||
std::map<std::string, std::string>::iterator itr;
|
||||
if ((itr = pairs.find(arg_name)) != pairs.end()) {
|
||||
|
||||
std::string s = itr->second;
|
||||
val = (char*) malloc(sizeof(char) * (s.length() + 1));
|
||||
std::strcpy(val, s.c_str());
|
||||
return true;
|
||||
} else {
|
||||
val = NULL;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
#endif //COMMAND_LINE_ARGS_H
|
138
thirdparty/bullet/src/Bullet3Common/b3FileUtils.h
vendored
Normal file
138
thirdparty/bullet/src/Bullet3Common/b3FileUtils.h
vendored
Normal file
|
@ -0,0 +1,138 @@
|
|||
#ifndef B3_FILE_UTILS_H
|
||||
#define B3_FILE_UTILS_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include "b3Scalar.h"
|
||||
#include <stddef.h>//ptrdiff_h
|
||||
#include <string.h>
|
||||
|
||||
struct b3FileUtils
|
||||
{
|
||||
b3FileUtils()
|
||||
{
|
||||
}
|
||||
virtual ~b3FileUtils()
|
||||
{
|
||||
}
|
||||
|
||||
static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
|
||||
{
|
||||
FILE* f=0;
|
||||
f = fopen(orgFileName,"rb");
|
||||
if (f)
|
||||
{
|
||||
//printf("original file found: [%s]\n", orgFileName);
|
||||
sprintf(relativeFileName,"%s", orgFileName);
|
||||
fclose(f);
|
||||
return true;
|
||||
}
|
||||
|
||||
//printf("Trying various directories, relative to current working directory\n");
|
||||
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
|
||||
int numPrefixes = sizeof(prefix)/sizeof(const char*);
|
||||
|
||||
f=0;
|
||||
bool fileFound = false;
|
||||
|
||||
for (int i=0;!f && i<numPrefixes;i++)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
|
||||
#else
|
||||
sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);
|
||||
#endif
|
||||
f = fopen(relativeFileName,"rb");
|
||||
if (f)
|
||||
{
|
||||
fileFound = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
return fileFound;
|
||||
}
|
||||
|
||||
static const char* strip2(const char* name, const char* pattern)
|
||||
{
|
||||
size_t const patlen = strlen(pattern);
|
||||
size_t patcnt = 0;
|
||||
const char * oriptr;
|
||||
const char * patloc;
|
||||
// find how many times the pattern occurs in the original string
|
||||
for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
|
||||
{
|
||||
patcnt++;
|
||||
}
|
||||
return oriptr;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static int extractPath(const char* fileName, char* path, int maxPathLength)
|
||||
{
|
||||
const char* stripped = strip2(fileName, "/");
|
||||
stripped = strip2(stripped, "\\");
|
||||
|
||||
ptrdiff_t len = stripped-fileName;
|
||||
b3Assert((len+1)<maxPathLength);
|
||||
|
||||
if (len && ((len+1)<maxPathLength))
|
||||
{
|
||||
|
||||
for (int i=0;i<len;i++)
|
||||
{
|
||||
path[i] = fileName[i];
|
||||
}
|
||||
path[len]=0;
|
||||
} else
|
||||
{
|
||||
len = 0;
|
||||
b3Assert(maxPathLength>0);
|
||||
if (maxPathLength>0)
|
||||
{
|
||||
path[len] = 0;
|
||||
}
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
static char toLowerChar(const char t)
|
||||
{
|
||||
if (t>=(char)'A' && t<=(char)'Z')
|
||||
return t + ((char)'a' - (char)'A');
|
||||
else
|
||||
return t;
|
||||
}
|
||||
|
||||
|
||||
static void toLower(char* str)
|
||||
{
|
||||
int len=strlen(str);
|
||||
for (int i=0;i<len;i++)
|
||||
{
|
||||
str[i] = toLowerChar(str[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*static const char* strip2(const char* name, const char* pattern)
|
||||
{
|
||||
size_t const patlen = strlen(pattern);
|
||||
size_t patcnt = 0;
|
||||
const char * oriptr;
|
||||
const char * patloc;
|
||||
// find how many times the pattern occurs in the original string
|
||||
for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
|
||||
{
|
||||
patcnt++;
|
||||
}
|
||||
return oriptr;
|
||||
}
|
||||
*/
|
||||
|
||||
};
|
||||
#endif //B3_FILE_UTILS_H
|
466
thirdparty/bullet/src/Bullet3Common/b3HashMap.h
vendored
Normal file
466
thirdparty/bullet/src/Bullet3Common/b3HashMap.h
vendored
Normal file
|
@ -0,0 +1,466 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef B3_HASH_MAP_H
|
||||
#define B3_HASH_MAP_H
|
||||
|
||||
#include "b3AlignedObjectArray.h"
|
||||
|
||||
|
||||
#include <string>
|
||||
|
||||
///very basic hashable string implementation, compatible with b3HashMap
|
||||
struct b3HashString
|
||||
{
|
||||
std::string m_string;
|
||||
unsigned int m_hash;
|
||||
|
||||
B3_FORCE_INLINE unsigned int getHash()const
|
||||
{
|
||||
return m_hash;
|
||||
}
|
||||
|
||||
|
||||
b3HashString(const char* name)
|
||||
:m_string(name)
|
||||
{
|
||||
|
||||
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
|
||||
static const unsigned int InitialFNV = 2166136261u;
|
||||
static const unsigned int FNVMultiple = 16777619u;
|
||||
|
||||
/* Fowler / Noll / Vo (FNV) Hash */
|
||||
unsigned int hash = InitialFNV;
|
||||
int len = m_string.length();
|
||||
for(int i = 0; i<len; i++)
|
||||
{
|
||||
hash = hash ^ (m_string[i]); /* xor the low 8 bits */
|
||||
hash = hash * FNVMultiple; /* multiply by the magic number */
|
||||
}
|
||||
m_hash = hash;
|
||||
}
|
||||
|
||||
int portableStringCompare(const char* src, const char* dst) const
|
||||
{
|
||||
int ret = 0 ;
|
||||
|
||||
while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
|
||||
++src, ++dst;
|
||||
|
||||
if ( ret < 0 )
|
||||
ret = -1 ;
|
||||
else if ( ret > 0 )
|
||||
ret = 1 ;
|
||||
|
||||
return( ret );
|
||||
}
|
||||
|
||||
bool equals(const b3HashString& other) const
|
||||
{
|
||||
return (m_string == other.m_string);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
const int B3_HASH_NULL=0xffffffff;
|
||||
|
||||
|
||||
class b3HashInt
|
||||
{
|
||||
int m_uid;
|
||||
public:
|
||||
b3HashInt(int uid) :m_uid(uid)
|
||||
{
|
||||
}
|
||||
|
||||
int getUid1() const
|
||||
{
|
||||
return m_uid;
|
||||
}
|
||||
|
||||
void setUid1(int uid)
|
||||
{
|
||||
m_uid = uid;
|
||||
}
|
||||
|
||||
bool equals(const b3HashInt& other) const
|
||||
{
|
||||
return getUid1() == other.getUid1();
|
||||
}
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash()const
|
||||
{
|
||||
int key = m_uid;
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
class b3HashPtr
|
||||
{
|
||||
|
||||
union
|
||||
{
|
||||
const void* m_pointer;
|
||||
int m_hashValues[2];
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
b3HashPtr(const void* ptr)
|
||||
:m_pointer(ptr)
|
||||
{
|
||||
}
|
||||
|
||||
const void* getPointer() const
|
||||
{
|
||||
return m_pointer;
|
||||
}
|
||||
|
||||
bool equals(const b3HashPtr& other) const
|
||||
{
|
||||
return getPointer() == other.getPointer();
|
||||
}
|
||||
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash()const
|
||||
{
|
||||
const bool VOID_IS_8 = ((sizeof(void*)==8));
|
||||
|
||||
int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
|
||||
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
template <class Value>
|
||||
class b3HashKeyPtr
|
||||
{
|
||||
int m_uid;
|
||||
public:
|
||||
|
||||
b3HashKeyPtr(int uid) :m_uid(uid)
|
||||
{
|
||||
}
|
||||
|
||||
int getUid1() const
|
||||
{
|
||||
return m_uid;
|
||||
}
|
||||
|
||||
bool equals(const b3HashKeyPtr<Value>& other) const
|
||||
{
|
||||
return getUid1() == other.getUid1();
|
||||
}
|
||||
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash()const
|
||||
{
|
||||
int key = m_uid;
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
template <class Value>
|
||||
class b3HashKey
|
||||
{
|
||||
int m_uid;
|
||||
public:
|
||||
|
||||
b3HashKey(int uid) :m_uid(uid)
|
||||
{
|
||||
}
|
||||
|
||||
int getUid1() const
|
||||
{
|
||||
return m_uid;
|
||||
}
|
||||
|
||||
bool equals(const b3HashKey<Value>& other) const
|
||||
{
|
||||
return getUid1() == other.getUid1();
|
||||
}
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash()const
|
||||
{
|
||||
int key = m_uid;
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
///The b3HashMap template class implements a generic and lightweight hashmap.
|
||||
///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
|
||||
template <class Key, class Value>
|
||||
class b3HashMap
|
||||
{
|
||||
|
||||
protected:
|
||||
b3AlignedObjectArray<int> m_hashTable;
|
||||
b3AlignedObjectArray<int> m_next;
|
||||
|
||||
b3AlignedObjectArray<Value> m_valueArray;
|
||||
b3AlignedObjectArray<Key> m_keyArray;
|
||||
|
||||
void growTables(const Key& /*key*/)
|
||||
{
|
||||
int newCapacity = m_valueArray.capacity();
|
||||
|
||||
if (m_hashTable.size() < newCapacity)
|
||||
{
|
||||
//grow hashtable and next table
|
||||
int curHashtableSize = m_hashTable.size();
|
||||
|
||||
m_hashTable.resize(newCapacity);
|
||||
m_next.resize(newCapacity);
|
||||
|
||||
int i;
|
||||
|
||||
for (i= 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_hashTable[i] = B3_HASH_NULL;
|
||||
}
|
||||
for (i = 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_next[i] = B3_HASH_NULL;
|
||||
}
|
||||
|
||||
for(i=0;i<curHashtableSize;i++)
|
||||
{
|
||||
//const Value& value = m_valueArray[i];
|
||||
//const Key& key = m_keyArray[i];
|
||||
|
||||
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask
|
||||
m_next[i] = m_hashTable[hashValue];
|
||||
m_hashTable[hashValue] = i;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
void insert(const Key& key, const Value& value) {
|
||||
int hash = key.getHash() & (m_valueArray.capacity()-1);
|
||||
|
||||
//replace value if the key is already there
|
||||
int index = findIndex(key);
|
||||
if (index != B3_HASH_NULL)
|
||||
{
|
||||
m_valueArray[index]=value;
|
||||
return;
|
||||
}
|
||||
|
||||
int count = m_valueArray.size();
|
||||
int oldCapacity = m_valueArray.capacity();
|
||||
m_valueArray.push_back(value);
|
||||
m_keyArray.push_back(key);
|
||||
|
||||
int newCapacity = m_valueArray.capacity();
|
||||
if (oldCapacity < newCapacity)
|
||||
{
|
||||
growTables(key);
|
||||
//hash with new capacity
|
||||
hash = key.getHash() & (m_valueArray.capacity()-1);
|
||||
}
|
||||
m_next[count] = m_hashTable[hash];
|
||||
m_hashTable[hash] = count;
|
||||
}
|
||||
|
||||
void remove(const Key& key) {
|
||||
|
||||
int hash = key.getHash() & (m_valueArray.capacity()-1);
|
||||
|
||||
int pairIndex = findIndex(key);
|
||||
|
||||
if (pairIndex ==B3_HASH_NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Remove the pair from the hash table.
|
||||
int index = m_hashTable[hash];
|
||||
b3Assert(index != B3_HASH_NULL);
|
||||
|
||||
int previous = B3_HASH_NULL;
|
||||
while (index != pairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_HASH_NULL)
|
||||
{
|
||||
b3Assert(m_next[previous] == pairIndex);
|
||||
m_next[previous] = m_next[pairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[hash] = m_next[pairIndex];
|
||||
}
|
||||
|
||||
// We now move the last pair into spot of the
|
||||
// pair being removed. We need to fix the hash
|
||||
// table indices to support the move.
|
||||
|
||||
int lastPairIndex = m_valueArray.size() - 1;
|
||||
|
||||
// If the removed pair is the last pair, we are done.
|
||||
if (lastPairIndex == pairIndex)
|
||||
{
|
||||
m_valueArray.pop_back();
|
||||
m_keyArray.pop_back();
|
||||
return;
|
||||
}
|
||||
|
||||
// Remove the last pair from the hash table.
|
||||
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
|
||||
|
||||
index = m_hashTable[lastHash];
|
||||
b3Assert(index != B3_HASH_NULL);
|
||||
|
||||
previous = B3_HASH_NULL;
|
||||
while (index != lastPairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_HASH_NULL)
|
||||
{
|
||||
b3Assert(m_next[previous] == lastPairIndex);
|
||||
m_next[previous] = m_next[lastPairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[lastHash] = m_next[lastPairIndex];
|
||||
}
|
||||
|
||||
// Copy the last pair into the remove pair's spot.
|
||||
m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
|
||||
m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
|
||||
|
||||
// Insert the last pair into the hash table
|
||||
m_next[pairIndex] = m_hashTable[lastHash];
|
||||
m_hashTable[lastHash] = pairIndex;
|
||||
|
||||
m_valueArray.pop_back();
|
||||
m_keyArray.pop_back();
|
||||
|
||||
}
|
||||
|
||||
|
||||
int size() const
|
||||
{
|
||||
return m_valueArray.size();
|
||||
}
|
||||
|
||||
const Value* getAtIndex(int index) const
|
||||
{
|
||||
b3Assert(index < m_valueArray.size());
|
||||
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
Value* getAtIndex(int index)
|
||||
{
|
||||
b3Assert(index < m_valueArray.size());
|
||||
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
Key getKeyAtIndex(int index)
|
||||
{
|
||||
b3Assert(index < m_keyArray.size());
|
||||
return m_keyArray[index];
|
||||
}
|
||||
|
||||
const Key getKeyAtIndex(int index) const
|
||||
{
|
||||
b3Assert(index < m_keyArray.size());
|
||||
return m_keyArray[index];
|
||||
}
|
||||
|
||||
Value* operator[](const Key& key) {
|
||||
return find(key);
|
||||
}
|
||||
|
||||
const Value* find(const Key& key) const
|
||||
{
|
||||
int index = findIndex(key);
|
||||
if (index == B3_HASH_NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
Value* find(const Key& key)
|
||||
{
|
||||
int index = findIndex(key);
|
||||
if (index == B3_HASH_NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
|
||||
int findIndex(const Key& key) const
|
||||
{
|
||||
unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
|
||||
|
||||
if (hash >= (unsigned int)m_hashTable.size())
|
||||
{
|
||||
return B3_HASH_NULL;
|
||||
}
|
||||
|
||||
int index = m_hashTable[hash];
|
||||
while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
|
||||
{
|
||||
index = m_next[index];
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
m_hashTable.clear();
|
||||
m_next.clear();
|
||||
m_valueArray.clear();
|
||||
m_keyArray.clear();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //B3_HASH_MAP_H
|
160
thirdparty/bullet/src/Bullet3Common/b3Logging.cpp
vendored
Normal file
160
thirdparty/bullet/src/Bullet3Common/b3Logging.cpp
vendored
Normal file
|
@ -0,0 +1,160 @@
|
|||
/*
|
||||
Copyright (c) 2013 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#include "b3Logging.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#endif //_WIN32
|
||||
|
||||
|
||||
void b3PrintfFuncDefault(const char* msg)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
OutputDebugStringA(msg);
|
||||
#endif
|
||||
printf("%s",msg);
|
||||
//is this portable?
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
void b3WarningMessageFuncDefault(const char* msg)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
OutputDebugStringA(msg);
|
||||
#endif
|
||||
printf("%s",msg);
|
||||
//is this portable?
|
||||
fflush(stdout);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3ErrorMessageFuncDefault(const char* msg)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
OutputDebugStringA(msg);
|
||||
#endif
|
||||
printf("%s",msg);
|
||||
|
||||
//is this portable?
|
||||
fflush(stdout);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
|
||||
static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
|
||||
static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
|
||||
|
||||
|
||||
///The developer can route b3Printf output using their own implementation
|
||||
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
|
||||
{
|
||||
b3s_printfFunc = printfFunc;
|
||||
}
|
||||
void b3SetCustomWarningMessageFunc(b3PrintfFunc* warningMessageFunc)
|
||||
{
|
||||
b3s_warningMessageFunc = warningMessageFunc;
|
||||
}
|
||||
void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
|
||||
{
|
||||
b3s_errorMessageFunc = errorMessageFunc;
|
||||
}
|
||||
|
||||
//#define B3_MAX_DEBUG_STRING_LENGTH 2048
|
||||
#define B3_MAX_DEBUG_STRING_LENGTH 32768
|
||||
|
||||
|
||||
void b3OutputPrintfVarArgsInternal(const char *str, ...)
|
||||
{
|
||||
char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
|
||||
va_list argList;
|
||||
va_start(argList, str);
|
||||
#ifdef _MSC_VER
|
||||
vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
|
||||
#else
|
||||
vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
|
||||
#endif
|
||||
(b3s_printfFunc)(strDebug);
|
||||
va_end(argList);
|
||||
}
|
||||
void b3OutputWarningMessageVarArgsInternal(const char *str, ...)
|
||||
{
|
||||
char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
|
||||
va_list argList;
|
||||
va_start(argList, str);
|
||||
#ifdef _MSC_VER
|
||||
vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
|
||||
#else
|
||||
vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
|
||||
#endif
|
||||
(b3s_warningMessageFunc)(strDebug);
|
||||
va_end(argList);
|
||||
}
|
||||
void b3OutputErrorMessageVarArgsInternal(const char *str, ...)
|
||||
{
|
||||
|
||||
char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
|
||||
va_list argList;
|
||||
va_start(argList, str);
|
||||
#ifdef _MSC_VER
|
||||
vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
|
||||
#else
|
||||
vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
|
||||
#endif
|
||||
(b3s_errorMessageFunc)(strDebug);
|
||||
va_end(argList);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3EnterProfileZoneDefault(const char* name)
|
||||
{
|
||||
}
|
||||
void b3LeaveProfileZoneDefault()
|
||||
{
|
||||
}
|
||||
static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
|
||||
static b3LeaveProfileZoneFunc* b3s_leaveFunc = b3LeaveProfileZoneDefault;
|
||||
void b3EnterProfileZone(const char* name)
|
||||
{
|
||||
(b3s_enterFunc)(name);
|
||||
}
|
||||
void b3LeaveProfileZone()
|
||||
{
|
||||
(b3s_leaveFunc)();
|
||||
}
|
||||
|
||||
void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc)
|
||||
{
|
||||
b3s_enterFunc = enterFunc;
|
||||
}
|
||||
void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
|
||||
{
|
||||
b3s_leaveFunc = leaveFunc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#ifndef _MSC_VER
|
||||
#undef vsprintf_s
|
||||
#endif
|
||||
|
77
thirdparty/bullet/src/Bullet3Common/b3Logging.h
vendored
Normal file
77
thirdparty/bullet/src/Bullet3Common/b3Logging.h
vendored
Normal file
|
@ -0,0 +1,77 @@
|
|||
|
||||
#ifndef B3_LOGGING_H
|
||||
#define B3_LOGGING_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
|
||||
///You can also customize the message by uncommenting out a different line below
|
||||
#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
|
||||
//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
|
||||
//#define b3Printf b3OutputPrintfVarArgsInternal
|
||||
//#define b3Printf(...) printf(__VA_ARGS__)
|
||||
//#define b3Printf(...)
|
||||
|
||||
#define b3Warning(...) do {b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n",__FILE__,__LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); }while(0)
|
||||
#define b3Error(...) do {b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n",__FILE__,__LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); } while(0)
|
||||
|
||||
|
||||
#ifndef B3_NO_PROFILE
|
||||
|
||||
void b3EnterProfileZone(const char* name);
|
||||
void b3LeaveProfileZone();
|
||||
#ifdef __cplusplus
|
||||
|
||||
class b3ProfileZone
|
||||
{
|
||||
public:
|
||||
b3ProfileZone(const char* name)
|
||||
{
|
||||
b3EnterProfileZone( name );
|
||||
}
|
||||
|
||||
~b3ProfileZone()
|
||||
{
|
||||
b3LeaveProfileZone();
|
||||
}
|
||||
};
|
||||
|
||||
#define B3_PROFILE( name ) b3ProfileZone __profile( name )
|
||||
#endif
|
||||
|
||||
#else //B3_NO_PROFILE
|
||||
|
||||
#define B3_PROFILE( name )
|
||||
#define b3StartProfile(a)
|
||||
#define b3StopProfile
|
||||
|
||||
#endif //#ifndef B3_NO_PROFILE
|
||||
|
||||
|
||||
typedef void (b3PrintfFunc)(const char* msg);
|
||||
typedef void (b3WarningMessageFunc)(const char* msg);
|
||||
typedef void (b3ErrorMessageFunc)(const char* msg);
|
||||
typedef void (b3EnterProfileZoneFunc)(const char* msg);
|
||||
typedef void (b3LeaveProfileZoneFunc)();
|
||||
|
||||
///The developer can route b3Printf output using their own implementation
|
||||
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
|
||||
void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
|
||||
void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
|
||||
|
||||
///Set custom profile zone functions (zones can be nested)
|
||||
void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
|
||||
void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
|
||||
|
||||
///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
|
||||
void b3OutputPrintfVarArgsInternal(const char *str, ...);
|
||||
void b3OutputWarningMessageVarArgsInternal(const char *str, ...);
|
||||
void b3OutputErrorMessageVarArgsInternal(const char *str, ...);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif//B3_LOGGING_H
|
1362
thirdparty/bullet/src/Bullet3Common/b3Matrix3x3.h
vendored
Normal file
1362
thirdparty/bullet/src/Bullet3Common/b3Matrix3x3.h
vendored
Normal file
File diff suppressed because it is too large
Load diff
71
thirdparty/bullet/src/Bullet3Common/b3MinMax.h
vendored
Normal file
71
thirdparty/bullet/src/Bullet3Common/b3MinMax.h
vendored
Normal file
|
@ -0,0 +1,71 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef B3_GEN_MINMAX_H
|
||||
#define B3_GEN_MINMAX_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
|
||||
{
|
||||
return a < b ? a : b ;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
|
||||
{
|
||||
return a > b ? a : b;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
|
||||
{
|
||||
return a < lb ? lb : (ub < a ? ub : a);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
|
||||
{
|
||||
if (b < a)
|
||||
{
|
||||
a = b;
|
||||
}
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
|
||||
{
|
||||
if (a < b)
|
||||
{
|
||||
a = b;
|
||||
}
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
|
||||
{
|
||||
if (a < lb)
|
||||
{
|
||||
a = lb;
|
||||
}
|
||||
else if (ub < a)
|
||||
{
|
||||
a = ub;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_GEN_MINMAX_H
|
121
thirdparty/bullet/src/Bullet3Common/b3PoolAllocator.h
vendored
Normal file
121
thirdparty/bullet/src/Bullet3Common/b3PoolAllocator.h
vendored
Normal file
|
@ -0,0 +1,121 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _BT_POOL_ALLOCATOR_H
|
||||
#define _BT_POOL_ALLOCATOR_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
|
||||
class b3PoolAllocator
|
||||
{
|
||||
int m_elemSize;
|
||||
int m_maxElements;
|
||||
int m_freeCount;
|
||||
void* m_firstFree;
|
||||
unsigned char* m_pool;
|
||||
|
||||
public:
|
||||
|
||||
b3PoolAllocator(int elemSize, int maxElements)
|
||||
:m_elemSize(elemSize),
|
||||
m_maxElements(maxElements)
|
||||
{
|
||||
m_pool = (unsigned char*) b3AlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
|
||||
|
||||
unsigned char* p = m_pool;
|
||||
m_firstFree = p;
|
||||
m_freeCount = m_maxElements;
|
||||
int count = m_maxElements;
|
||||
while (--count) {
|
||||
*(void**)p = (p + m_elemSize);
|
||||
p += m_elemSize;
|
||||
}
|
||||
*(void**)p = 0;
|
||||
}
|
||||
|
||||
~b3PoolAllocator()
|
||||
{
|
||||
b3AlignedFree( m_pool);
|
||||
}
|
||||
|
||||
int getFreeCount() const
|
||||
{
|
||||
return m_freeCount;
|
||||
}
|
||||
|
||||
int getUsedCount() const
|
||||
{
|
||||
return m_maxElements - m_freeCount;
|
||||
}
|
||||
|
||||
int getMaxCount() const
|
||||
{
|
||||
return m_maxElements;
|
||||
}
|
||||
|
||||
void* allocate(int size)
|
||||
{
|
||||
// release mode fix
|
||||
(void)size;
|
||||
b3Assert(!size || size<=m_elemSize);
|
||||
b3Assert(m_freeCount>0);
|
||||
void* result = m_firstFree;
|
||||
m_firstFree = *(void**)m_firstFree;
|
||||
--m_freeCount;
|
||||
return result;
|
||||
}
|
||||
|
||||
bool validPtr(void* ptr)
|
||||
{
|
||||
if (ptr) {
|
||||
if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void freeMemory(void* ptr)
|
||||
{
|
||||
if (ptr) {
|
||||
b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
|
||||
|
||||
*(void**)ptr = m_firstFree;
|
||||
m_firstFree = ptr;
|
||||
++m_freeCount;
|
||||
}
|
||||
}
|
||||
|
||||
int getElementSize() const
|
||||
{
|
||||
return m_elemSize;
|
||||
}
|
||||
|
||||
unsigned char* getPoolAddress()
|
||||
{
|
||||
return m_pool;
|
||||
}
|
||||
|
||||
const unsigned char* getPoolAddress() const
|
||||
{
|
||||
return m_pool;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //_BT_POOL_ALLOCATOR_H
|
245
thirdparty/bullet/src/Bullet3Common/b3QuadWord.h
vendored
Normal file
245
thirdparty/bullet/src/Bullet3Common/b3QuadWord.h
vendored
Normal file
|
@ -0,0 +1,245 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef B3_SIMD_QUADWORD_H
|
||||
#define B3_SIMD_QUADWORD_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
#include "b3MinMax.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if defined (__CELLOS_LV2) && defined (__SPU__)
|
||||
#include <altivec.h>
|
||||
#endif
|
||||
|
||||
/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion.
|
||||
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
|
||||
*/
|
||||
#ifndef USE_LIBSPE2
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3QuadWord
|
||||
#else
|
||||
class b3QuadWord
|
||||
#endif
|
||||
{
|
||||
protected:
|
||||
|
||||
#if defined (__SPU__) && defined (__CELLOS_LV2__)
|
||||
union {
|
||||
vec_float4 mVec128;
|
||||
b3Scalar m_floats[4];
|
||||
};
|
||||
public:
|
||||
vec_float4 get128() const
|
||||
{
|
||||
return mVec128;
|
||||
}
|
||||
|
||||
#else //__CELLOS_LV2__ __SPU__
|
||||
|
||||
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
|
||||
public:
|
||||
union {
|
||||
b3SimdFloat4 mVec128;
|
||||
b3Scalar m_floats[4];
|
||||
struct {b3Scalar x,y,z,w;};
|
||||
};
|
||||
public:
|
||||
B3_FORCE_INLINE b3SimdFloat4 get128() const
|
||||
{
|
||||
return mVec128;
|
||||
}
|
||||
B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
|
||||
{
|
||||
mVec128 = v128;
|
||||
}
|
||||
#else
|
||||
public:
|
||||
union
|
||||
{
|
||||
b3Scalar m_floats[4];
|
||||
struct {b3Scalar x,y,z,w;};
|
||||
};
|
||||
#endif // B3_USE_SSE
|
||||
|
||||
#endif //__CELLOS_LV2__ __SPU__
|
||||
|
||||
public:
|
||||
|
||||
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
|
||||
|
||||
// Set Vector
|
||||
B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
|
||||
{
|
||||
mVec128 = vec;
|
||||
}
|
||||
|
||||
// Copy constructor
|
||||
B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
|
||||
{
|
||||
mVec128 = rhs.mVec128;
|
||||
}
|
||||
|
||||
// Assignment Operator
|
||||
B3_FORCE_INLINE b3QuadWord&
|
||||
operator=(const b3QuadWord& v)
|
||||
{
|
||||
mVec128 = v.mVec128;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/**@brief Return the x value */
|
||||
B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
|
||||
/**@brief Return the y value */
|
||||
B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
|
||||
/**@brief Return the z value */
|
||||
B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
|
||||
/**@brief Set the x value */
|
||||
B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x;};
|
||||
/**@brief Set the y value */
|
||||
B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y;};
|
||||
/**@brief Set the z value */
|
||||
B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z;};
|
||||
/**@brief Set the w value */
|
||||
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;};
|
||||
/**@brief Return the x value */
|
||||
|
||||
|
||||
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
|
||||
//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
|
||||
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
|
||||
B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; }
|
||||
B3_FORCE_INLINE operator const b3Scalar *() const { return &m_floats[0]; }
|
||||
|
||||
B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
|
||||
{
|
||||
#ifdef B3_USE_SSE
|
||||
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
|
||||
#else
|
||||
return ((m_floats[3]==other.m_floats[3]) &&
|
||||
(m_floats[2]==other.m_floats[2]) &&
|
||||
(m_floats[1]==other.m_floats[1]) &&
|
||||
(m_floats[0]==other.m_floats[0]));
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
|
||||
{
|
||||
return !(*this == other);
|
||||
}
|
||||
|
||||
/**@brief Set x,y,z and zero w
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
*/
|
||||
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
|
||||
{
|
||||
m_floats[0]=_x;
|
||||
m_floats[1]=_y;
|
||||
m_floats[2]=_z;
|
||||
m_floats[3] = 0.f;
|
||||
}
|
||||
|
||||
/* void getValue(b3Scalar *m) const
|
||||
{
|
||||
m[0] = m_floats[0];
|
||||
m[1] = m_floats[1];
|
||||
m[2] = m_floats[2];
|
||||
}
|
||||
*/
|
||||
/**@brief Set the values
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
* @param w Value of w
|
||||
*/
|
||||
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
|
||||
{
|
||||
m_floats[0]=_x;
|
||||
m_floats[1]=_y;
|
||||
m_floats[2]=_z;
|
||||
m_floats[3]=_w;
|
||||
}
|
||||
/**@brief No initialization constructor */
|
||||
B3_FORCE_INLINE b3QuadWord()
|
||||
// :m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
|
||||
{
|
||||
}
|
||||
|
||||
/**@brief Three argument constructor (zeros w)
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
*/
|
||||
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
|
||||
{
|
||||
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
|
||||
}
|
||||
|
||||
/**@brief Initializing constructor
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
* @param w Value of w
|
||||
*/
|
||||
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
|
||||
{
|
||||
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
|
||||
}
|
||||
|
||||
/**@brief Set each element to the max of the current values and the values of another b3QuadWord
|
||||
* @param other The other b3QuadWord to compare with
|
||||
*/
|
||||
B3_FORCE_INLINE void setMax(const b3QuadWord& other)
|
||||
{
|
||||
#ifdef B3_USE_SSE
|
||||
mVec128 = _mm_max_ps(mVec128, other.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vmaxq_f32(mVec128, other.mVec128);
|
||||
#else
|
||||
b3SetMax(m_floats[0], other.m_floats[0]);
|
||||
b3SetMax(m_floats[1], other.m_floats[1]);
|
||||
b3SetMax(m_floats[2], other.m_floats[2]);
|
||||
b3SetMax(m_floats[3], other.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
/**@brief Set each element to the min of the current values and the values of another b3QuadWord
|
||||
* @param other The other b3QuadWord to compare with
|
||||
*/
|
||||
B3_FORCE_INLINE void setMin(const b3QuadWord& other)
|
||||
{
|
||||
#ifdef B3_USE_SSE
|
||||
mVec128 = _mm_min_ps(mVec128, other.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vminq_f32(mVec128, other.mVec128);
|
||||
#else
|
||||
b3SetMin(m_floats[0], other.m_floats[0]);
|
||||
b3SetMin(m_floats[1], other.m_floats[1]);
|
||||
b3SetMin(m_floats[2], other.m_floats[2]);
|
||||
b3SetMin(m_floats[3], other.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //B3_SIMD_QUADWORD_H
|
918
thirdparty/bullet/src/Bullet3Common/b3Quaternion.h
vendored
Normal file
918
thirdparty/bullet/src/Bullet3Common/b3Quaternion.h
vendored
Normal file
|
@ -0,0 +1,918 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef B3_SIMD__QUATERNION_H_
|
||||
#define B3_SIMD__QUATERNION_H_
|
||||
|
||||
|
||||
#include "b3Vector3.h"
|
||||
#include "b3QuadWord.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef B3_USE_SSE
|
||||
|
||||
const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
|
||||
|
||||
const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
|
||||
const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
|
||||
|
||||
#endif
|
||||
|
||||
/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
|
||||
class b3Quaternion : public b3QuadWord {
|
||||
public:
|
||||
/**@brief No initialization constructor */
|
||||
b3Quaternion() {}
|
||||
|
||||
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))|| defined(B3_USE_NEON)
|
||||
// Set Vector
|
||||
B3_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec)
|
||||
{
|
||||
mVec128 = vec;
|
||||
}
|
||||
|
||||
// Copy constructor
|
||||
B3_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs)
|
||||
{
|
||||
mVec128 = rhs.mVec128;
|
||||
}
|
||||
|
||||
// Assignment Operator
|
||||
B3_FORCE_INLINE b3Quaternion&
|
||||
operator=(const b3Quaternion& v)
|
||||
{
|
||||
mVec128 = v.mVec128;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// template <typename b3Scalar>
|
||||
// explicit Quaternion(const b3Scalar *v) : Tuple4<b3Scalar>(v) {}
|
||||
/**@brief Constructor from scalars */
|
||||
b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
|
||||
: b3QuadWord(_x, _y, _z, _w)
|
||||
{
|
||||
//b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
|
||||
}
|
||||
/**@brief Axis angle Constructor
|
||||
* @param axis The axis which the rotation is around
|
||||
* @param angle The magnitude of the rotation around the angle (Radians) */
|
||||
b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle)
|
||||
{
|
||||
setRotation(_axis, _angle);
|
||||
}
|
||||
/**@brief Constructor from Euler angles
|
||||
* @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z
|
||||
* @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y
|
||||
* @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */
|
||||
b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
|
||||
{
|
||||
#ifndef B3_EULER_DEFAULT_ZYX
|
||||
setEuler(yaw, pitch, roll);
|
||||
#else
|
||||
setEulerZYX(yaw, pitch, roll);
|
||||
#endif
|
||||
}
|
||||
/**@brief Set the rotation using axis angle notation
|
||||
* @param axis The axis around which to rotate
|
||||
* @param angle The magnitude of the rotation in Radians */
|
||||
void setRotation(const b3Vector3& axis, const b3Scalar& _angle)
|
||||
{
|
||||
b3Scalar d = axis.length();
|
||||
b3Assert(d != b3Scalar(0.0));
|
||||
b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
|
||||
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
|
||||
b3Cos(_angle * b3Scalar(0.5)));
|
||||
}
|
||||
/**@brief Set the quaternion using Euler angles
|
||||
* @param yaw Angle around Y
|
||||
* @param pitch Angle around X
|
||||
* @param roll Angle around Z */
|
||||
void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
|
||||
{
|
||||
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
|
||||
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
|
||||
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
|
||||
b3Scalar cosYaw = b3Cos(halfYaw);
|
||||
b3Scalar sinYaw = b3Sin(halfYaw);
|
||||
b3Scalar cosPitch = b3Cos(halfPitch);
|
||||
b3Scalar sinPitch = b3Sin(halfPitch);
|
||||
b3Scalar cosRoll = b3Cos(halfRoll);
|
||||
b3Scalar sinRoll = b3Sin(halfRoll);
|
||||
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
|
||||
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
|
||||
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
|
||||
}
|
||||
|
||||
/**@brief Set the quaternion using euler angles
|
||||
* @param yaw Angle around Z
|
||||
* @param pitch Angle around Y
|
||||
* @param roll Angle around X */
|
||||
void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
|
||||
{
|
||||
b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
|
||||
b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
|
||||
b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
|
||||
b3Scalar cosYaw = b3Cos(halfYaw);
|
||||
b3Scalar sinYaw = b3Sin(halfYaw);
|
||||
b3Scalar cosPitch = b3Cos(halfPitch);
|
||||
b3Scalar sinPitch = b3Sin(halfPitch);
|
||||
b3Scalar cosRoll = b3Cos(halfRoll);
|
||||
b3Scalar sinRoll = b3Sin(halfRoll);
|
||||
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
|
||||
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
|
||||
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
|
||||
normalize();
|
||||
}
|
||||
|
||||
/**@brief Get the euler angles from this quaternion
|
||||
* @param yaw Angle around Z
|
||||
* @param pitch Angle around Y
|
||||
* @param roll Angle around X */
|
||||
void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
|
||||
{
|
||||
b3Scalar squ;
|
||||
b3Scalar sqx;
|
||||
b3Scalar sqy;
|
||||
b3Scalar sqz;
|
||||
b3Scalar sarg;
|
||||
sqx = m_floats[0] * m_floats[0];
|
||||
sqy = m_floats[1] * m_floats[1];
|
||||
sqz = m_floats[2] * m_floats[2];
|
||||
squ = m_floats[3] * m_floats[3];
|
||||
rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
|
||||
sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
|
||||
pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
|
||||
yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
|
||||
}
|
||||
|
||||
/**@brief Add two quaternions
|
||||
* @param q The quaternion to add to this one */
|
||||
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
mVec128 = _mm_add_ps(mVec128, q.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vaddq_f32(mVec128, q.mVec128);
|
||||
#else
|
||||
m_floats[0] += q.getX();
|
||||
m_floats[1] += q.getY();
|
||||
m_floats[2] += q.getZ();
|
||||
m_floats[3] += q.m_floats[3];
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Subtract out a quaternion
|
||||
* @param q The quaternion to subtract from this one */
|
||||
b3Quaternion& operator-=(const b3Quaternion& q)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
mVec128 = _mm_sub_ps(mVec128, q.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vsubq_f32(mVec128, q.mVec128);
|
||||
#else
|
||||
m_floats[0] -= q.getX();
|
||||
m_floats[1] -= q.getY();
|
||||
m_floats[2] -= q.getZ();
|
||||
m_floats[3] -= q.m_floats[3];
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Scale this quaternion
|
||||
* @param s The scalar to scale by */
|
||||
b3Quaternion& operator*=(const b3Scalar& s)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
|
||||
vs = b3_pshufd_ps(vs, 0); // (S S S S)
|
||||
mVec128 = _mm_mul_ps(mVec128, vs);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vmulq_n_f32(mVec128, s);
|
||||
#else
|
||||
m_floats[0] *= s;
|
||||
m_floats[1] *= s;
|
||||
m_floats[2] *= s;
|
||||
m_floats[3] *= s;
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Multiply this quaternion by q on the right
|
||||
* @param q The other quaternion
|
||||
* Equivilant to this = this * q */
|
||||
b3Quaternion& operator*=(const b3Quaternion& q)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vQ2 = q.get128();
|
||||
|
||||
__m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0,1,2,0));
|
||||
__m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0));
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
__m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1,2,0,1));
|
||||
__m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1));
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2,0,1,2));
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2));
|
||||
|
||||
B1 = B1 * B2; // A3 *= B3
|
||||
|
||||
mVec128 = b3_splat_ps(mVec128, 3); // A0
|
||||
mVec128 = mVec128 * vQ2; // A0 * B0
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
mVec128 = mVec128 - B1; // AB03 = AB0 - AB3
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
mVec128 = mVec128+ A1; // AB03 + AB12
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = mVec128;
|
||||
float32x4_t vQ2 = q.get128();
|
||||
float32x4_t A0, A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
|
||||
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
}
|
||||
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
|
||||
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
A0 = vaddq_f32(A0, A1); // AB03 + AB12
|
||||
|
||||
mVec128 = A0;
|
||||
#else
|
||||
setValue(
|
||||
m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(),
|
||||
m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(),
|
||||
m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(),
|
||||
m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ());
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
/**@brief Return the dot product between this quaternion and another
|
||||
* @param q The other quaternion */
|
||||
b3Scalar dot(const b3Quaternion& q) const
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vd;
|
||||
|
||||
vd = _mm_mul_ps(mVec128, q.mVec128);
|
||||
|
||||
__m128 t = _mm_movehl_ps(vd, vd);
|
||||
vd = _mm_add_ps(vd, t);
|
||||
t = _mm_shuffle_ps(vd, vd, 0x55);
|
||||
vd = _mm_add_ss(vd, t);
|
||||
|
||||
return _mm_cvtss_f32(vd);
|
||||
#elif defined(B3_USE_NEON)
|
||||
float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
|
||||
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
|
||||
x = vpadd_f32(x, x);
|
||||
return vget_lane_f32(x, 0);
|
||||
#else
|
||||
return m_floats[0] * q.getX() +
|
||||
m_floats[1] * q.getY() +
|
||||
m_floats[2] * q.getZ() +
|
||||
m_floats[3] * q.m_floats[3];
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the length squared of the quaternion */
|
||||
b3Scalar length2() const
|
||||
{
|
||||
return dot(*this);
|
||||
}
|
||||
|
||||
/**@brief Return the length of the quaternion */
|
||||
b3Scalar length() const
|
||||
{
|
||||
return b3Sqrt(length2());
|
||||
}
|
||||
|
||||
/**@brief Normalize the quaternion
|
||||
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
|
||||
b3Quaternion& normalize()
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vd;
|
||||
|
||||
vd = _mm_mul_ps(mVec128, mVec128);
|
||||
|
||||
__m128 t = _mm_movehl_ps(vd, vd);
|
||||
vd = _mm_add_ps(vd, t);
|
||||
t = _mm_shuffle_ps(vd, vd, 0x55);
|
||||
vd = _mm_add_ss(vd, t);
|
||||
|
||||
vd = _mm_sqrt_ss(vd);
|
||||
vd = _mm_div_ss(b3vOnes, vd);
|
||||
vd = b3_pshufd_ps(vd, 0); // splat
|
||||
mVec128 = _mm_mul_ps(mVec128, vd);
|
||||
|
||||
return *this;
|
||||
#else
|
||||
return *this /= length();
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return a scaled version of this quaternion
|
||||
* @param s The scale factor */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Scalar& s) const
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
|
||||
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
|
||||
|
||||
return b3Quaternion(_mm_mul_ps(mVec128, vs));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion(vmulq_n_f32(mVec128, s));
|
||||
#else
|
||||
return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return an inversely scaled versionof this quaternion
|
||||
* @param s The inverse scale factor */
|
||||
b3Quaternion operator/(const b3Scalar& s) const
|
||||
{
|
||||
b3Assert(s != b3Scalar(0.0));
|
||||
return *this * (b3Scalar(1.0) / s);
|
||||
}
|
||||
|
||||
/**@brief Inversely scale this quaternion
|
||||
* @param s The scale factor */
|
||||
b3Quaternion& operator/=(const b3Scalar& s)
|
||||
{
|
||||
b3Assert(s != b3Scalar(0.0));
|
||||
return *this *= b3Scalar(1.0) / s;
|
||||
}
|
||||
|
||||
/**@brief Return a normalized version of this quaternion */
|
||||
b3Quaternion normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
/**@brief Return the angle between this quaternion and the other
|
||||
* @param q The other quaternion */
|
||||
b3Scalar angle(const b3Quaternion& q) const
|
||||
{
|
||||
b3Scalar s = b3Sqrt(length2() * q.length2());
|
||||
b3Assert(s != b3Scalar(0.0));
|
||||
return b3Acos(dot(q) / s);
|
||||
}
|
||||
/**@brief Return the angle of rotation represented by this quaternion */
|
||||
b3Scalar getAngle() const
|
||||
{
|
||||
b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]);
|
||||
return s;
|
||||
}
|
||||
|
||||
/**@brief Return the axis of the rotation represented by this quaternion */
|
||||
b3Vector3 getAxis() const
|
||||
{
|
||||
b3Scalar s_squared = 1.f-m_floats[3]*m_floats[3];
|
||||
|
||||
if (s_squared < b3Scalar(10.) * B3_EPSILON) //Check for divide by zero
|
||||
return b3MakeVector3(1.0, 0.0, 0.0); // Arbitrary
|
||||
b3Scalar s = 1.f/b3Sqrt(s_squared);
|
||||
return b3MakeVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
|
||||
}
|
||||
|
||||
/**@brief Return the inverse of this quaternion */
|
||||
b3Quaternion inverse() const
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
return b3Quaternion(_mm_xor_ps(mVec128, b3vQInv));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vQInv));
|
||||
#else
|
||||
return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the sum of this quaternion and the other
|
||||
* @param q2 The other quaternion */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator+(const b3Quaternion& q2) const
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
|
||||
#else
|
||||
const b3Quaternion& q1 = *this;
|
||||
return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the difference between this quaternion and the other
|
||||
* @param q2 The other quaternion */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator-(const b3Quaternion& q2) const
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
|
||||
#else
|
||||
const b3Quaternion& q1 = *this;
|
||||
return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the negative of this quaternion
|
||||
* This simply negates each element */
|
||||
B3_FORCE_INLINE b3Quaternion operator-() const
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask) );
|
||||
#else
|
||||
const b3Quaternion& q2 = *this;
|
||||
return b3Quaternion( - q2.getX(), - q2.getY(), - q2.getZ(), - q2.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
/**@todo document this and it's use */
|
||||
B3_FORCE_INLINE b3Quaternion farthest( const b3Quaternion& qd) const
|
||||
{
|
||||
b3Quaternion diff,sum;
|
||||
diff = *this - qd;
|
||||
sum = *this + qd;
|
||||
if( diff.dot(diff) > sum.dot(sum) )
|
||||
return qd;
|
||||
return (-qd);
|
||||
}
|
||||
|
||||
/**@todo document this and it's use */
|
||||
B3_FORCE_INLINE b3Quaternion nearest( const b3Quaternion& qd) const
|
||||
{
|
||||
b3Quaternion diff,sum;
|
||||
diff = *this - qd;
|
||||
sum = *this + qd;
|
||||
if( diff.dot(diff) < sum.dot(sum) )
|
||||
return qd;
|
||||
return (-qd);
|
||||
}
|
||||
|
||||
|
||||
/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
|
||||
* @param q The other quaternion to interpolate with
|
||||
* @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
|
||||
* Slerp interpolates assuming constant velocity. */
|
||||
b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
|
||||
{
|
||||
b3Scalar magnitude = b3Sqrt(length2() * q.length2());
|
||||
b3Assert(magnitude > b3Scalar(0));
|
||||
|
||||
b3Scalar product = dot(q) / magnitude;
|
||||
if (b3Fabs(product) < b3Scalar(1))
|
||||
{
|
||||
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
|
||||
const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
|
||||
|
||||
const b3Scalar theta = b3Acos(sign * product);
|
||||
const b3Scalar s1 = b3Sin(sign * t * theta);
|
||||
const b3Scalar d = b3Scalar(1.0) / b3Sin(theta);
|
||||
const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta);
|
||||
|
||||
return b3Quaternion(
|
||||
(m_floats[0] * s0 + q.getX() * s1) * d,
|
||||
(m_floats[1] * s0 + q.getY() * s1) * d,
|
||||
(m_floats[2] * s0 + q.getZ() * s1) * d,
|
||||
(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
|
||||
}
|
||||
else
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
}
|
||||
|
||||
static const b3Quaternion& getIdentity()
|
||||
{
|
||||
static const b3Quaternion identityQuat(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.),b3Scalar(1.));
|
||||
return identityQuat;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**@brief Return the product of two quaternions */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Quaternion& q1, const b3Quaternion& q2)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vQ1 = q1.get128();
|
||||
__m128 vQ2 = q2.get128();
|
||||
__m128 A0, A1, B1, A2, B2;
|
||||
|
||||
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0,1,2,0)); // X Y z x // vtrn
|
||||
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); // W W W X // vdup vext
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1)); // Y Z X Y // vext
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn
|
||||
|
||||
B1 = B1 * B2; // A3 *= B3
|
||||
|
||||
A0 = b3_splat_ps(vQ1, 3); // A0
|
||||
A0 = A0 * vQ2; // A0 * B0
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
A0 = A0 - B1; // AB03 = AB0 - AB3
|
||||
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
A0 = A0 + A1; // AB03 + AB12
|
||||
|
||||
return b3Quaternion(A0);
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = q1.get128();
|
||||
float32x4_t vQ2 = q2.get128();
|
||||
float32x4_t A0, A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
|
||||
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
}
|
||||
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
|
||||
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
A0 = vaddq_f32(A0, A1); // AB03 + AB12
|
||||
|
||||
return b3Quaternion(A0);
|
||||
|
||||
#else
|
||||
return b3Quaternion(
|
||||
q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
|
||||
q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
|
||||
q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
|
||||
q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Quaternion& q, const b3Vector3& w)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vQ1 = q.get128();
|
||||
__m128 vQ2 = w.get128();
|
||||
__m128 A1, B1, A2, B2, A3, B3;
|
||||
|
||||
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3,3,3,0));
|
||||
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0,1,2,0));
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1));
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1));
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2));
|
||||
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2));
|
||||
|
||||
A3 = A3 * B3; // A3 *= B3
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
A1 = A1 - A3; // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = q.get128();
|
||||
float32x4_t vQ2 = w.get128();
|
||||
float32x4_t A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
|
||||
|
||||
vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
|
||||
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
}
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X
|
||||
B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
|
||||
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#else
|
||||
return b3Quaternion(
|
||||
q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
|
||||
q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
|
||||
q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
|
||||
-q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Vector3& w, const b3Quaternion& q)
|
||||
{
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
__m128 vQ1 = w.get128();
|
||||
__m128 vQ2 = q.get128();
|
||||
__m128 A1, B1, A2, B2, A3, B3;
|
||||
|
||||
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0,1,2,0)); // X Y z x
|
||||
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); // W W W X
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1));
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1));
|
||||
|
||||
A2 = A2 *B2;
|
||||
|
||||
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2));
|
||||
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2));
|
||||
|
||||
A3 = A3 * B3; // A3 *= B3
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
A1 = A1 - A3; // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = w.get128();
|
||||
float32x4_t vQ2 = q.get128();
|
||||
float32x4_t A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
|
||||
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
|
||||
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
}
|
||||
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
|
||||
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
|
||||
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#else
|
||||
return b3Quaternion(
|
||||
+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
|
||||
+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
|
||||
+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
|
||||
-w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Calculate the dot product between two quaternions */
|
||||
B3_FORCE_INLINE b3Scalar
|
||||
b3Dot(const b3Quaternion& q1, const b3Quaternion& q2)
|
||||
{
|
||||
return q1.dot(q2);
|
||||
}
|
||||
|
||||
|
||||
/**@brief Return the length of a quaternion */
|
||||
B3_FORCE_INLINE b3Scalar
|
||||
b3Length(const b3Quaternion& q)
|
||||
{
|
||||
return q.length();
|
||||
}
|
||||
|
||||
/**@brief Return the angle between two quaternions*/
|
||||
B3_FORCE_INLINE b3Scalar
|
||||
b3Angle(const b3Quaternion& q1, const b3Quaternion& q2)
|
||||
{
|
||||
return q1.angle(q2);
|
||||
}
|
||||
|
||||
/**@brief Return the inverse of a quaternion*/
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3Inverse(const b3Quaternion& q)
|
||||
{
|
||||
return q.inverse();
|
||||
}
|
||||
|
||||
/**@brief Return the result of spherical linear interpolation betwen two quaternions
|
||||
* @param q1 The first quaternion
|
||||
* @param q2 The second quaternion
|
||||
* @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
|
||||
* Slerp assumes constant velocity between positions. */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3Slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t)
|
||||
{
|
||||
return q1.slerp(q2, t);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1)
|
||||
{
|
||||
return rot0*rot1;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3QuatNormalized(const b3Quaternion& orn)
|
||||
{
|
||||
return orn.normalized();
|
||||
}
|
||||
|
||||
|
||||
|
||||
B3_FORCE_INLINE b3Vector3
|
||||
b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
|
||||
{
|
||||
b3Quaternion q = rotation * v;
|
||||
q *= rotation.inverse();
|
||||
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
|
||||
return b3MakeVector3(_mm_and_ps(q.get128(), b3vFFF0fMask));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask));
|
||||
#else
|
||||
return b3MakeVector3(q.getX(),q.getY(),q.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
|
||||
{
|
||||
b3Vector3 c = v0.cross(v1);
|
||||
b3Scalar d = v0.dot(v1);
|
||||
|
||||
if (d < -1.0 + B3_EPSILON)
|
||||
{
|
||||
b3Vector3 n,unused;
|
||||
b3PlaneSpace1(v0,n,unused);
|
||||
return b3Quaternion(n.getX(),n.getY(),n.getZ(),0.0f); // just pick any vector that is orthogonal to v0
|
||||
}
|
||||
|
||||
b3Scalar s = b3Sqrt((1.0f + d) * 2.0f);
|
||||
b3Scalar rs = 1.0f / s;
|
||||
|
||||
return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
|
||||
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3ShortestArcQuatNormalize2(b3Vector3& v0,b3Vector3& v1)
|
||||
{
|
||||
v0.normalize();
|
||||
v1.normalize();
|
||||
return b3ShortestArcQuat(v0,v1);
|
||||
}
|
||||
|
||||
#endif //B3_SIMD__QUATERNION_H_
|
||||
|
||||
|
||||
|
Some files were not shown because too many files have changed in this diff Show more
Loading…
Reference in a new issue