added pgs/jacobi cpu solver making the gpu rigid body pipeline work again (aside from running the solver on CPU)
This commit is contained in:
erwin coumans
@@ -15,6 +15,7 @@ premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_sat/kernels/sat.
|
||||
premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_sat/kernels/satClipHullContacts.cl" --headerfile="../opencl/gpu_sat/kernels/satClipHullContacts.h" --stringname="satClipKernelsCL" stringify
|
||||
|
||||
premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/integrateKernel.cl" --headerfile="../opencl/gpu_rigidbody/kernels/integrateKernel.h" --stringname="integrateKernelCL" stringify
|
||||
premake4 --file=stringifyKernel.lua --kernelfile="../opencl/gpu_rigidbody/kernels/updateAabbsKernel.cl" --headerfile="../opencl/gpu_rigidbody/kernels/updateAabbsKernel.h" --stringname="updateAabbsKernelCL" stringify
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -42,7 +42,7 @@ void GpuDemo::initCL(int preferredDeviceIndex, int preferredPlatformIndex)
|
||||
//#ifdef CL_PLATFORM_INTEL
|
||||
//cl_device_type deviceType = CL_DEVICE_TYPE_ALL;
|
||||
//#else
|
||||
cl_device_type deviceType = CL_DEVICE_TYPE_CPU;
|
||||
cl_device_type deviceType = CL_DEVICE_TYPE_GPU;
|
||||
//#endif
|
||||
|
||||
cl_platform_id platformId;
|
||||
|
||||
@@ -34,9 +34,9 @@ public:
|
||||
:useOpenCL(false),//true),
|
||||
preferredOpenCLPlatformIndex(-1),
|
||||
preferredOpenCLDeviceIndex(-1),
|
||||
arraySizeX(2),
|
||||
arraySizeY(2 ),
|
||||
arraySizeZ(2),
|
||||
arraySizeX(22),
|
||||
arraySizeY(22 ),
|
||||
arraySizeZ(22),
|
||||
m_useConcaveMesh(false),
|
||||
gapX(4.3),
|
||||
gapY(4.0),
|
||||
|
||||
@@ -129,13 +129,15 @@ void GpuRigidBodyDemo::initPhysics(const ConstructionInfo& ci)
|
||||
|
||||
int colIndex = np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
|
||||
|
||||
float mass = 1.f;
|
||||
|
||||
for (int i=0;i<ci.arraySizeX;i++)
|
||||
{
|
||||
for (int j=0;j<ci.arraySizeY;j++)
|
||||
{
|
||||
for (int k=0;k<ci.arraySizeZ;k++)
|
||||
{
|
||||
float mass = i==0? 0.f : 1.f;
|
||||
|
||||
btVector3 position(k*3,i*3,j*3);
|
||||
btQuaternion orn(1,0,0,0);
|
||||
|
||||
@@ -149,6 +151,7 @@ void GpuRigidBodyDemo::initPhysics(const ConstructionInfo& ci)
|
||||
}
|
||||
}
|
||||
np->writeAllBodiesToGpu();
|
||||
bp->writeAabbsToGpu();
|
||||
}
|
||||
|
||||
|
||||
@@ -223,29 +226,13 @@ void GpuRigidBodyDemo::clientMoveAndDisplay()
|
||||
|
||||
{
|
||||
int ciErrNum = 0;
|
||||
|
||||
|
||||
ciErrNum = 0;//clSetKernelArg(fpio.m_copyTransformsToVBOKernel, 2, sizeof(cl_mem), (void*)&fpio.m_clObjectsBuffer);
|
||||
|
||||
cl_mem bodies = m_data->m_rigidBodyPipeline->getBodyBuffer();
|
||||
btLauncherCL launch(m_clData->m_clQueue,m_data->m_copyTransformsToVBOKernel);
|
||||
launch.setBuffer(bodies);
|
||||
launch.setBuffer(m_data->m_instancePosOrnColor->getBufferCL());
|
||||
launch.setConst(numObjects);
|
||||
|
||||
launch.launch1D(numObjects);
|
||||
//ciErrNum = clSetKernelArg(fpio.m_copyTransformsToVBOKernel, 3, sizeof(cl_mem), (void*)&bodies);
|
||||
//ciErrNum = clSetKernelArg(fpio.m_copyTransformsToVBOKernel, 1, sizeof(int), &fpio.m_numObjects);
|
||||
|
||||
if (numObjects)
|
||||
{
|
||||
size_t workGroupSize = 64;
|
||||
size_t numWorkItems = workGroupSize*((numObjects+ (workGroupSize)) / workGroupSize);
|
||||
|
||||
//ciErrNum = clEnqueueNDRangeKernel(fpio.m_cqCommandQue, fpio.m_copyTransformsToVBOKernel, 1, NULL, &numWorkItems, &workGroupSize,0 ,0 ,0);
|
||||
oclCHECKERROR(ciErrNum, CL_SUCCESS);
|
||||
}
|
||||
|
||||
oclCHECKERROR(ciErrNum, CL_SUCCESS);
|
||||
}
|
||||
|
||||
if (animate)
|
||||
@@ -254,7 +241,6 @@ void GpuRigidBodyDemo::clientMoveAndDisplay()
|
||||
assert(err==GL_NO_ERROR);
|
||||
m_data->m_instancePosOrnColor->copyToHostPointer(positions,3*numObjects,0);
|
||||
glUnmapBuffer( GL_ARRAY_BUFFER);
|
||||
|
||||
err = glGetError();
|
||||
assert(err==GL_NO_ERROR);
|
||||
}
|
||||
|
||||
270
opencl/gpu_rigidbody/host/btBroadphaseProxy.h
Normal file
270
opencl/gpu_rigidbody/host/btBroadphaseProxy.h
Normal file
@@ -0,0 +1,270 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_BROADPHASE_PROXY_H
|
||||
#define BT_BROADPHASE_PROXY_H
|
||||
|
||||
#include "BulletCommon/btScalar.h" //for SIMD_FORCE_INLINE
|
||||
#include "BulletCommon/btVector3.h"
|
||||
#include "BulletCommon/btAlignedAllocator.h"
|
||||
|
||||
|
||||
/// btDispatcher uses these types
|
||||
/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
|
||||
/// to facilitate type checking
|
||||
/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code
|
||||
enum BroadphaseNativeTypes
|
||||
{
|
||||
// polyhedral convex shapes
|
||||
BOX_SHAPE_PROXYTYPE,
|
||||
TRIANGLE_SHAPE_PROXYTYPE,
|
||||
TETRAHEDRAL_SHAPE_PROXYTYPE,
|
||||
CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
|
||||
CONVEX_HULL_SHAPE_PROXYTYPE,
|
||||
CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
|
||||
CUSTOM_POLYHEDRAL_SHAPE_TYPE,
|
||||
//implicit convex shapes
|
||||
IMPLICIT_CONVEX_SHAPES_START_HERE,
|
||||
SPHERE_SHAPE_PROXYTYPE,
|
||||
MULTI_SPHERE_SHAPE_PROXYTYPE,
|
||||
CAPSULE_SHAPE_PROXYTYPE,
|
||||
CONE_SHAPE_PROXYTYPE,
|
||||
CONVEX_SHAPE_PROXYTYPE,
|
||||
CYLINDER_SHAPE_PROXYTYPE,
|
||||
UNIFORM_SCALING_SHAPE_PROXYTYPE,
|
||||
MINKOWSKI_SUM_SHAPE_PROXYTYPE,
|
||||
MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
|
||||
BOX_2D_SHAPE_PROXYTYPE,
|
||||
CONVEX_2D_SHAPE_PROXYTYPE,
|
||||
CUSTOM_CONVEX_SHAPE_TYPE,
|
||||
//concave shapes
|
||||
CONCAVE_SHAPES_START_HERE,
|
||||
//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
|
||||
TRIANGLE_MESH_SHAPE_PROXYTYPE,
|
||||
SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE,
|
||||
///used for demo integration FAST/Swift collision library and Bullet
|
||||
FAST_CONCAVE_MESH_PROXYTYPE,
|
||||
//terrain
|
||||
TERRAIN_SHAPE_PROXYTYPE,
|
||||
///Used for GIMPACT Trimesh integration
|
||||
GIMPACT_SHAPE_PROXYTYPE,
|
||||
///Multimaterial mesh
|
||||
MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
|
||||
|
||||
EMPTY_SHAPE_PROXYTYPE,
|
||||
STATIC_PLANE_PROXYTYPE,
|
||||
CUSTOM_CONCAVE_SHAPE_TYPE,
|
||||
CONCAVE_SHAPES_END_HERE,
|
||||
|
||||
COMPOUND_SHAPE_PROXYTYPE,
|
||||
|
||||
SOFTBODY_SHAPE_PROXYTYPE,
|
||||
HFFLUID_SHAPE_PROXYTYPE,
|
||||
HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE,
|
||||
INVALID_SHAPE_PROXYTYPE,
|
||||
|
||||
MAX_BROADPHASE_COLLISION_TYPES
|
||||
|
||||
};
|
||||
|
||||
|
||||
///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
|
||||
///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
|
||||
ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
|
||||
{
|
||||
|
||||
BT_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 btCollisionObject or Rigidbody class
|
||||
void* m_clientObject;
|
||||
short int m_collisionFilterGroup;
|
||||
short int m_collisionFilterMask;
|
||||
void* m_multiSapParentProxy;
|
||||
int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
|
||||
|
||||
btVector3 m_aabbMin;
|
||||
btVector3 m_aabbMax;
|
||||
|
||||
SIMD_FORCE_INLINE int getUid() const
|
||||
{
|
||||
return m_uniqueId;
|
||||
}
|
||||
|
||||
//used for memory pools
|
||||
btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
|
||||
{
|
||||
}
|
||||
|
||||
btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
|
||||
:m_clientObject(userPtr),
|
||||
m_collisionFilterGroup(collisionFilterGroup),
|
||||
m_collisionFilterMask(collisionFilterMask),
|
||||
m_aabbMin(aabbMin),
|
||||
m_aabbMax(aabbMax)
|
||||
{
|
||||
m_multiSapParentProxy = multiSapParentProxy;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
|
||||
{
|
||||
return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE);
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isConvex(int proxyType)
|
||||
{
|
||||
return (proxyType < CONCAVE_SHAPES_START_HERE);
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isNonMoving(int proxyType)
|
||||
{
|
||||
return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE));
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isConcave(int proxyType)
|
||||
{
|
||||
return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
|
||||
(proxyType < CONCAVE_SHAPES_END_HERE));
|
||||
}
|
||||
static SIMD_FORCE_INLINE bool isCompound(int proxyType)
|
||||
{
|
||||
return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isSoftBody(int proxyType)
|
||||
{
|
||||
return (proxyType == SOFTBODY_SHAPE_PROXYTYPE);
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
|
||||
{
|
||||
return (proxyType == STATIC_PLANE_PROXYTYPE);
|
||||
}
|
||||
|
||||
static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
|
||||
{
|
||||
return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
;
|
||||
|
||||
class btCollisionAlgorithm;
|
||||
|
||||
struct btBroadphaseProxy;
|
||||
|
||||
|
||||
|
||||
///The btBroadphasePair class contains a pair of aabb-overlapping objects.
|
||||
///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
|
||||
ATTRIBUTE_ALIGNED16(struct) btBroadphasePair
|
||||
{
|
||||
btBroadphasePair ()
|
||||
:
|
||||
m_pProxy0(0),
|
||||
m_pProxy1(0),
|
||||
m_algorithm(0),
|
||||
m_internalInfo1(0)
|
||||
{
|
||||
}
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btBroadphasePair(const btBroadphasePair& other)
|
||||
: m_pProxy0(other.m_pProxy0),
|
||||
m_pProxy1(other.m_pProxy1),
|
||||
m_algorithm(other.m_algorithm),
|
||||
m_internalInfo1(other.m_internalInfo1)
|
||||
{
|
||||
}
|
||||
btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
|
||||
{
|
||||
|
||||
//keep them sorted, so the std::set operations work
|
||||
if (proxy0.m_uniqueId < proxy1.m_uniqueId)
|
||||
{
|
||||
m_pProxy0 = &proxy0;
|
||||
m_pProxy1 = &proxy1;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_pProxy0 = &proxy1;
|
||||
m_pProxy1 = &proxy0;
|
||||
}
|
||||
|
||||
m_algorithm = 0;
|
||||
m_internalInfo1 = 0;
|
||||
|
||||
}
|
||||
|
||||
btBroadphaseProxy* m_pProxy0;
|
||||
btBroadphaseProxy* m_pProxy1;
|
||||
|
||||
mutable btCollisionAlgorithm* m_algorithm;
|
||||
union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version.
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
//comparison for set operation, see Solid DT_Encounter
|
||||
SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b)
|
||||
{
|
||||
return a.m_pProxy0 < b.m_pProxy0 ||
|
||||
(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
class btBroadphasePairSortPredicate
|
||||
{
|
||||
public:
|
||||
|
||||
bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const
|
||||
{
|
||||
const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
|
||||
const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
|
||||
const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
|
||||
const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
|
||||
|
||||
return uidA0 > uidB0 ||
|
||||
(a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
|
||||
(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b)
|
||||
{
|
||||
return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
|
||||
}
|
||||
|
||||
|
||||
#endif //BT_BROADPHASE_PROXY_H
|
||||
|
||||
534
opencl/gpu_rigidbody/host/btCollisionObject.h
Normal file
534
opencl/gpu_rigidbody/host/btCollisionObject.h
Normal file
@@ -0,0 +1,534 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_COLLISION_OBJECT_H
|
||||
#define BT_COLLISION_OBJECT_H
|
||||
|
||||
#include "BulletCommon/btTransform.h"
|
||||
|
||||
//island management, m_activationState1
|
||||
#define ACTIVE_TAG 1
|
||||
#define ISLAND_SLEEPING 2
|
||||
#define WANTS_DEACTIVATION 3
|
||||
#define DISABLE_DEACTIVATION 4
|
||||
#define DISABLE_SIMULATION 5
|
||||
|
||||
struct btBroadphaseProxy;
|
||||
class btCollisionShape;
|
||||
struct btCollisionShapeData;
|
||||
#include "BulletCommon/btMotionState.h"
|
||||
#include "BulletCommon/btAlignedAllocator.h"
|
||||
#include "BulletCommon/btAlignedObjectArray.h"
|
||||
|
||||
typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btCollisionObjectData btCollisionObjectDoubleData
|
||||
#define btCollisionObjectDataName "btCollisionObjectDoubleData"
|
||||
#else
|
||||
#define btCollisionObjectData btCollisionObjectFloatData
|
||||
#define btCollisionObjectDataName "btCollisionObjectFloatData"
|
||||
#endif
|
||||
|
||||
|
||||
/// btCollisionObject can be used to manage collision detection objects.
|
||||
/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
|
||||
/// They can be added to the btCollisionWorld.
|
||||
ATTRIBUTE_ALIGNED16(class) btCollisionObject
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btTransform m_worldTransform;
|
||||
|
||||
///m_interpolationWorldTransform is used for CCD and interpolation
|
||||
///it can be either previous or future (predicted) transform
|
||||
btTransform m_interpolationWorldTransform;
|
||||
//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
|
||||
//without destroying the continuous interpolated motion (which uses this interpolation velocities)
|
||||
btVector3 m_interpolationLinearVelocity;
|
||||
btVector3 m_interpolationAngularVelocity;
|
||||
|
||||
btVector3 m_anisotropicFriction;
|
||||
int m_hasAnisotropicFriction;
|
||||
btScalar m_contactProcessingThreshold;
|
||||
|
||||
btBroadphaseProxy* m_broadphaseHandle;
|
||||
btCollisionShape* m_collisionShape;
|
||||
///m_extensionPointer is used by some internal low-level Bullet extensions.
|
||||
void* m_extensionPointer;
|
||||
|
||||
///m_rootCollisionShape is temporarily used to store the original collision shape
|
||||
///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes
|
||||
///If it is NULL, the m_collisionShape is not temporarily replaced.
|
||||
btCollisionShape* m_rootCollisionShape;
|
||||
|
||||
int m_collisionFlags;
|
||||
|
||||
int m_islandTag1;
|
||||
int m_companionId;
|
||||
|
||||
mutable int m_activationState1;
|
||||
mutable btScalar m_deactivationTime;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_restitution;
|
||||
btScalar m_rollingFriction;
|
||||
|
||||
///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
|
||||
///do not assign your own m_internalType unless you write a new dynamics object class.
|
||||
int m_internalType;
|
||||
|
||||
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
|
||||
void* m_userObjectPointer;
|
||||
|
||||
///time of impact calculation
|
||||
btScalar m_hitFraction;
|
||||
|
||||
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
|
||||
btScalar m_ccdSweptSphereRadius;
|
||||
|
||||
/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
|
||||
btScalar m_ccdMotionThreshold;
|
||||
|
||||
/// If some object should have elaborate collision filtering by sub-classes
|
||||
int m_checkCollideWith;
|
||||
|
||||
virtual bool checkCollideWithOverride(const btCollisionObject* /* co */) const
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
enum CollisionFlags
|
||||
{
|
||||
CF_STATIC_OBJECT= 1,
|
||||
CF_KINEMATIC_OBJECT= 2,
|
||||
CF_NO_CONTACT_RESPONSE = 4,
|
||||
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
|
||||
CF_CHARACTER_OBJECT = 16,
|
||||
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
|
||||
CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
|
||||
};
|
||||
|
||||
enum CollisionObjectTypes
|
||||
{
|
||||
CO_COLLISION_OBJECT =1,
|
||||
CO_RIGID_BODY=2,
|
||||
///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
|
||||
///It is useful for collision sensors, explosion objects, character controller etc.
|
||||
CO_GHOST_OBJECT=4,
|
||||
CO_SOFT_BODY=8,
|
||||
CO_HF_FLUID=16,
|
||||
CO_USER_TYPE=32
|
||||
};
|
||||
|
||||
enum AnisotropicFrictionFlags
|
||||
{
|
||||
CF_ANISOTROPIC_FRICTION_DISABLED=0,
|
||||
CF_ANISOTROPIC_FRICTION = 1,
|
||||
CF_ANISOTROPIC_ROLLING_FRICTION = 2
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE bool mergesSimulationIslands() const
|
||||
{
|
||||
///static objects, kinematic and object without contact response don't merge islands
|
||||
return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
|
||||
}
|
||||
|
||||
const btVector3& getAnisotropicFriction() const
|
||||
{
|
||||
return m_anisotropicFriction;
|
||||
}
|
||||
void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
|
||||
{
|
||||
m_anisotropicFriction = anisotropicFriction;
|
||||
bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
|
||||
m_hasAnisotropicFriction = isUnity?frictionMode : 0;
|
||||
}
|
||||
bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const
|
||||
{
|
||||
return (m_hasAnisotropicFriction&frictionMode)!=0;
|
||||
}
|
||||
|
||||
///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
|
||||
///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
|
||||
void setContactProcessingThreshold( btScalar contactProcessingThreshold)
|
||||
{
|
||||
m_contactProcessingThreshold = contactProcessingThreshold;
|
||||
}
|
||||
btScalar getContactProcessingThreshold() const
|
||||
{
|
||||
return m_contactProcessingThreshold;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isStaticObject() const {
|
||||
return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isKinematicObject() const
|
||||
{
|
||||
return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
|
||||
{
|
||||
return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool hasContactResponse() const {
|
||||
return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
|
||||
}
|
||||
|
||||
|
||||
btCollisionObject();
|
||||
|
||||
virtual ~btCollisionObject();
|
||||
|
||||
virtual void setCollisionShape(btCollisionShape* collisionShape)
|
||||
{
|
||||
m_collisionShape = collisionShape;
|
||||
m_rootCollisionShape = collisionShape;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
|
||||
{
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
|
||||
{
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
///Avoid using this internal API call, the extension pointer is used by some Bullet extensions.
|
||||
///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
|
||||
void* internalGetExtensionPointer() const
|
||||
{
|
||||
return m_extensionPointer;
|
||||
}
|
||||
///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
|
||||
///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
|
||||
void internalSetExtensionPointer(void* pointer)
|
||||
{
|
||||
m_extensionPointer = pointer;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;}
|
||||
|
||||
void setActivationState(int newState) const;
|
||||
|
||||
void setDeactivationTime(btScalar time)
|
||||
{
|
||||
m_deactivationTime = time;
|
||||
}
|
||||
btScalar getDeactivationTime() const
|
||||
{
|
||||
return m_deactivationTime;
|
||||
}
|
||||
|
||||
void forceActivationState(int newState) const;
|
||||
|
||||
void activate(bool forceActivation = false) const;
|
||||
|
||||
SIMD_FORCE_INLINE bool isActive() const
|
||||
{
|
||||
return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
|
||||
}
|
||||
|
||||
void setRestitution(btScalar rest)
|
||||
{
|
||||
m_restitution = rest;
|
||||
}
|
||||
btScalar getRestitution() const
|
||||
{
|
||||
return m_restitution;
|
||||
}
|
||||
void setFriction(btScalar frict)
|
||||
{
|
||||
m_friction = frict;
|
||||
}
|
||||
btScalar getFriction() const
|
||||
{
|
||||
return m_friction;
|
||||
}
|
||||
|
||||
void setRollingFriction(btScalar frict)
|
||||
{
|
||||
m_rollingFriction = frict;
|
||||
}
|
||||
btScalar getRollingFriction() const
|
||||
{
|
||||
return m_rollingFriction;
|
||||
}
|
||||
|
||||
|
||||
///reserved for Bullet internal usage
|
||||
int getInternalType() const
|
||||
{
|
||||
return m_internalType;
|
||||
}
|
||||
|
||||
btTransform& getWorldTransform()
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
const btTransform& getWorldTransform() const
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
void setWorldTransform(const btTransform& worldTrans)
|
||||
{
|
||||
m_worldTransform = worldTrans;
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle()
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
|
||||
void setBroadphaseHandle(btBroadphaseProxy* handle)
|
||||
{
|
||||
m_broadphaseHandle = handle;
|
||||
}
|
||||
|
||||
|
||||
const btTransform& getInterpolationWorldTransform() const
|
||||
{
|
||||
return m_interpolationWorldTransform;
|
||||
}
|
||||
|
||||
btTransform& getInterpolationWorldTransform()
|
||||
{
|
||||
return m_interpolationWorldTransform;
|
||||
}
|
||||
|
||||
void setInterpolationWorldTransform(const btTransform& trans)
|
||||
{
|
||||
m_interpolationWorldTransform = trans;
|
||||
}
|
||||
|
||||
void setInterpolationLinearVelocity(const btVector3& linvel)
|
||||
{
|
||||
m_interpolationLinearVelocity = linvel;
|
||||
}
|
||||
|
||||
void setInterpolationAngularVelocity(const btVector3& angvel)
|
||||
{
|
||||
m_interpolationAngularVelocity = angvel;
|
||||
}
|
||||
|
||||
const btVector3& getInterpolationLinearVelocity() const
|
||||
{
|
||||
return m_interpolationLinearVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getInterpolationAngularVelocity() const
|
||||
{
|
||||
return m_interpolationAngularVelocity;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int getIslandTag() const
|
||||
{
|
||||
return m_islandTag1;
|
||||
}
|
||||
|
||||
void setIslandTag(int tag)
|
||||
{
|
||||
m_islandTag1 = tag;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE int getCompanionId() const
|
||||
{
|
||||
return m_companionId;
|
||||
}
|
||||
|
||||
void setCompanionId(int id)
|
||||
{
|
||||
m_companionId = id;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar getHitFraction() const
|
||||
{
|
||||
return m_hitFraction;
|
||||
}
|
||||
|
||||
void setHitFraction(btScalar hitFraction)
|
||||
{
|
||||
m_hitFraction = hitFraction;
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int getCollisionFlags() const
|
||||
{
|
||||
return m_collisionFlags;
|
||||
}
|
||||
|
||||
void setCollisionFlags(int flags)
|
||||
{
|
||||
m_collisionFlags = flags;
|
||||
}
|
||||
|
||||
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
|
||||
btScalar getCcdSweptSphereRadius() const
|
||||
{
|
||||
return m_ccdSweptSphereRadius;
|
||||
}
|
||||
|
||||
///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
|
||||
void setCcdSweptSphereRadius(btScalar radius)
|
||||
{
|
||||
m_ccdSweptSphereRadius = radius;
|
||||
}
|
||||
|
||||
btScalar getCcdMotionThreshold() const
|
||||
{
|
||||
return m_ccdMotionThreshold;
|
||||
}
|
||||
|
||||
btScalar getCcdSquareMotionThreshold() const
|
||||
{
|
||||
return m_ccdMotionThreshold*m_ccdMotionThreshold;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
|
||||
void setCcdMotionThreshold(btScalar ccdMotionThreshold)
|
||||
{
|
||||
m_ccdMotionThreshold = ccdMotionThreshold;
|
||||
}
|
||||
|
||||
///users can point to their objects, userPointer is not used by Bullet
|
||||
void* getUserPointer() const
|
||||
{
|
||||
return m_userObjectPointer;
|
||||
}
|
||||
|
||||
///users can point to their objects, userPointer is not used by Bullet
|
||||
void setUserPointer(void* userPointer)
|
||||
{
|
||||
m_userObjectPointer = userPointer;
|
||||
}
|
||||
|
||||
|
||||
inline bool checkCollideWith(const btCollisionObject* co) const
|
||||
{
|
||||
if (m_checkCollideWith)
|
||||
return checkCollideWithOverride(co);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
|
||||
|
||||
virtual void serializeSingleObject(class btSerializer* serializer) const;
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btCollisionObjectDoubleData
|
||||
{
|
||||
void *m_broadphaseHandle;
|
||||
void *m_collisionShape;
|
||||
btCollisionShapeData *m_rootCollisionShape;
|
||||
char *m_name;
|
||||
|
||||
btTransformDoubleData m_worldTransform;
|
||||
btTransformDoubleData m_interpolationWorldTransform;
|
||||
btVector3DoubleData m_interpolationLinearVelocity;
|
||||
btVector3DoubleData m_interpolationAngularVelocity;
|
||||
btVector3DoubleData m_anisotropicFriction;
|
||||
double m_contactProcessingThreshold;
|
||||
double m_deactivationTime;
|
||||
double m_friction;
|
||||
double m_rollingFriction;
|
||||
double m_restitution;
|
||||
double m_hitFraction;
|
||||
double m_ccdSweptSphereRadius;
|
||||
double m_ccdMotionThreshold;
|
||||
|
||||
int m_hasAnisotropicFriction;
|
||||
int m_collisionFlags;
|
||||
int m_islandTag1;
|
||||
int m_companionId;
|
||||
int m_activationState1;
|
||||
int m_internalType;
|
||||
int m_checkCollideWith;
|
||||
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btCollisionObjectFloatData
|
||||
{
|
||||
void *m_broadphaseHandle;
|
||||
void *m_collisionShape;
|
||||
btCollisionShapeData *m_rootCollisionShape;
|
||||
char *m_name;
|
||||
|
||||
btTransformFloatData m_worldTransform;
|
||||
btTransformFloatData m_interpolationWorldTransform;
|
||||
btVector3FloatData m_interpolationLinearVelocity;
|
||||
btVector3FloatData m_interpolationAngularVelocity;
|
||||
btVector3FloatData m_anisotropicFriction;
|
||||
float m_contactProcessingThreshold;
|
||||
float m_deactivationTime;
|
||||
float m_friction;
|
||||
float m_rollingFriction;
|
||||
|
||||
float m_restitution;
|
||||
float m_hitFraction;
|
||||
float m_ccdSweptSphereRadius;
|
||||
float m_ccdMotionThreshold;
|
||||
|
||||
int m_hasAnisotropicFriction;
|
||||
int m_collisionFlags;
|
||||
int m_islandTag1;
|
||||
int m_companionId;
|
||||
int m_activationState1;
|
||||
int m_internalType;
|
||||
int m_checkCollideWith;
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btCollisionObjectData);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //BT_COLLISION_OBJECT_H
|
||||
52
opencl/gpu_rigidbody/host/btConstraintSolver.h
Normal file
52
opencl/gpu_rigidbody/host/btConstraintSolver.h
Normal file
@@ -0,0 +1,52 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_CONSTRAINT_SOLVER_H
|
||||
#define BT_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "BulletCommon/btScalar.h"
|
||||
|
||||
class btPersistentManifold;
|
||||
class btRigidBody;
|
||||
class btCollisionObject;
|
||||
class btTypedConstraint;
|
||||
struct btContactSolverInfo;
|
||||
struct btBroadphaseProxy;
|
||||
class btIDebugDraw;
|
||||
class btStackAlloc;
|
||||
class btDispatcher;
|
||||
/// btConstraintSolver provides solver interface
|
||||
class btConstraintSolver
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
virtual ~btConstraintSolver() {}
|
||||
|
||||
virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
|
||||
|
||||
///solve a group of constraints
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
|
||||
|
||||
virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset() = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_CONSTRAINT_SOLVER_H
|
||||
159
opencl/gpu_rigidbody/host/btContactSolverInfo.h
Normal file
159
opencl/gpu_rigidbody/host/btContactSolverInfo.h
Normal file
@@ -0,0 +1,159 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_CONTACT_SOLVER_INFO
|
||||
#define BT_CONTACT_SOLVER_INFO
|
||||
|
||||
#include "BulletCommon/btScalar.h"
|
||||
|
||||
enum btSolverMode
|
||||
{
|
||||
SOLVER_RANDMIZE_ORDER = 1,
|
||||
SOLVER_FRICTION_SEPARATE = 2,
|
||||
SOLVER_USE_WARMSTARTING = 4,
|
||||
SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
|
||||
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
|
||||
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
|
||||
SOLVER_CACHE_FRIENDLY = 128,
|
||||
SOLVER_SIMD = 256,
|
||||
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
|
||||
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
|
||||
};
|
||||
|
||||
struct btContactSolverInfoData
|
||||
{
|
||||
|
||||
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
btScalar m_friction;
|
||||
btScalar m_timeStep;
|
||||
btScalar m_restitution;
|
||||
int m_numIterations;
|
||||
btScalar m_maxErrorReduction;
|
||||
btScalar m_sor;
|
||||
btScalar m_erp;//used as Baumgarte factor
|
||||
btScalar m_erp2;//used in Split Impulse
|
||||
btScalar m_globalCfm;//constraint force mixing
|
||||
int m_splitImpulse;
|
||||
btScalar m_splitImpulsePenetrationThreshold;
|
||||
btScalar m_splitImpulseTurnErp;
|
||||
btScalar m_linearSlop;
|
||||
btScalar m_warmstartingFactor;
|
||||
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
btScalar m_maxGyroscopicForce;
|
||||
btScalar m_singleAxisRollingFrictionThreshold;
|
||||
|
||||
|
||||
};
|
||||
|
||||
struct btContactSolverInfo : public btContactSolverInfoData
|
||||
{
|
||||
|
||||
|
||||
|
||||
inline btContactSolverInfo()
|
||||
{
|
||||
m_tau = btScalar(0.6);
|
||||
m_damping = btScalar(1.0);
|
||||
m_friction = btScalar(0.3);
|
||||
m_timeStep = btScalar(1.f/60.f);
|
||||
m_restitution = btScalar(0.);
|
||||
m_maxErrorReduction = btScalar(20.);
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.2);
|
||||
m_erp2 = btScalar(0.8);
|
||||
m_globalCfm = btScalar(0.);
|
||||
m_sor = btScalar(1.);
|
||||
m_splitImpulse = true;
|
||||
m_splitImpulsePenetrationThreshold = -.04f;
|
||||
m_splitImpulseTurnErp = 0.1f;
|
||||
m_linearSlop = btScalar(0.0);
|
||||
m_warmstartingFactor=btScalar(0.85);
|
||||
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
|
||||
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
|
||||
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
|
||||
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
|
||||
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
|
||||
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
|
||||
}
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btContactSolverInfoDoubleData
|
||||
{
|
||||
double m_tau;
|
||||
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
double m_friction;
|
||||
double m_timeStep;
|
||||
double m_restitution;
|
||||
double m_maxErrorReduction;
|
||||
double m_sor;
|
||||
double m_erp;//used as Baumgarte factor
|
||||
double m_erp2;//used in Split Impulse
|
||||
double m_globalCfm;//constraint force mixing
|
||||
double m_splitImpulsePenetrationThreshold;
|
||||
double m_splitImpulseTurnErp;
|
||||
double m_linearSlop;
|
||||
double m_warmstartingFactor;
|
||||
double m_maxGyroscopicForce;
|
||||
double m_singleAxisRollingFrictionThreshold;
|
||||
|
||||
int m_numIterations;
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
int m_splitImpulse;
|
||||
char m_padding[4];
|
||||
|
||||
};
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btContactSolverInfoFloatData
|
||||
{
|
||||
float m_tau;
|
||||
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
float m_friction;
|
||||
float m_timeStep;
|
||||
|
||||
float m_restitution;
|
||||
float m_maxErrorReduction;
|
||||
float m_sor;
|
||||
float m_erp;//used as Baumgarte factor
|
||||
|
||||
float m_erp2;//used in Split Impulse
|
||||
float m_globalCfm;//constraint force mixing
|
||||
float m_splitImpulsePenetrationThreshold;
|
||||
float m_splitImpulseTurnErp;
|
||||
|
||||
float m_linearSlop;
|
||||
float m_warmstartingFactor;
|
||||
float m_maxGyroscopicForce;
|
||||
float m_singleAxisRollingFrictionThreshold;
|
||||
|
||||
int m_numIterations;
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
|
||||
int m_splitImpulse;
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //BT_CONTACT_SOLVER_INFO
|
||||
@@ -365,6 +365,10 @@ cl_mem btGpuNarrowPhase::getCollidablesGpu()
|
||||
return m_data->m_collidablesGPU->getBufferCL();
|
||||
}
|
||||
|
||||
cl_mem btGpuNarrowPhase::getAabbBufferGpu()
|
||||
{
|
||||
return m_data->m_localShapeAABBGPU->getBufferCL();
|
||||
}
|
||||
int btGpuNarrowPhase::getNumCollidablesGpu() const
|
||||
{
|
||||
return m_data->m_collidablesGPU->size();
|
||||
@@ -386,6 +390,41 @@ cl_mem btGpuNarrowPhase::getContactsGpu()
|
||||
|
||||
void btGpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbs, int numObjects)
|
||||
{
|
||||
int nContactOut = 0;
|
||||
|
||||
int maxTriConvexPairCapacity = 8192;
|
||||
btOpenCLArray<btInt4> triangleConvexPairs(m_context,m_queue, maxTriConvexPairCapacity);
|
||||
int numTriConvexPairsOut=0;
|
||||
|
||||
btOpenCLArray<btInt2> broadphasePairsGPU(m_context,m_queue);
|
||||
broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs);
|
||||
btOpenCLArray<btYetAnotherAabb> clAabbArray(this->m_context,this->m_queue);
|
||||
clAabbArray.setFromOpenCLBuffer(aabbs,numObjects);
|
||||
|
||||
m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT(
|
||||
&broadphasePairsGPU, numBroadphasePairs,
|
||||
m_data->m_bodyBufferGPU,
|
||||
m_data->m_pBufContactOutGPU,
|
||||
nContactOut,
|
||||
*m_data->m_convexPolyhedraGPU,
|
||||
*m_data->m_convexVerticesGPU,
|
||||
*m_data->m_uniqueEdgesGPU,
|
||||
*m_data->m_convexFacesGPU,
|
||||
*m_data->m_convexIndicesGPU,
|
||||
*m_data->m_collidablesGPU,
|
||||
*m_data->m_gpuChildShapes,
|
||||
clAabbArray,
|
||||
*m_data->m_worldVertsB1GPU,
|
||||
*m_data->m_clippingFacesOutGPU,
|
||||
*m_data->m_worldNormalsAGPU,
|
||||
*m_data->m_worldVertsA1GPU,
|
||||
*m_data->m_worldVertsB2GPU,
|
||||
|
||||
numObjects,
|
||||
maxTriConvexPairCapacity,
|
||||
triangleConvexPairs,
|
||||
numTriConvexPairsOut
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
@@ -415,7 +454,7 @@ int btGpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const f
|
||||
body.m_frictionCoeff = friction;
|
||||
body.m_restituitionCoeff = restitution;
|
||||
body.m_angVel.setZero();
|
||||
body.m_linVel.setValue(0,-1,0);//.setZero();
|
||||
body.m_linVel.setValue(0,0,0);//.setZero();
|
||||
body.m_pos.setValue(position[0],position[1],position[2]);
|
||||
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
|
||||
body.m_collidableIdx = collidableIndex;
|
||||
@@ -500,5 +539,6 @@ void btGpuNarrowPhase::writeAllBodiesToGpu()
|
||||
m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies);
|
||||
|
||||
m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -64,6 +64,9 @@ public:
|
||||
cl_mem getContactsGpu();
|
||||
int getNumContactsGpu() const;
|
||||
|
||||
cl_mem getAabbBufferGpu();
|
||||
|
||||
|
||||
int allocateCollidable();
|
||||
|
||||
btCollidable& getCollidableCpu(int collidableIndex);
|
||||
|
||||
@@ -1,12 +1,18 @@
|
||||
#include "btGpuRigidBodyPipeline.h"
|
||||
#include "btGpuRigidBodyPipelineInternalData.h"
|
||||
#include "../kernels/integrateKernel.h"
|
||||
#include "../kernels/updateAabbsKernel.h"
|
||||
|
||||
#include "../../basic_initialize/btOpenCLUtils.h"
|
||||
#include "btGpuNarrowPhase.h"
|
||||
#include "BulletGeometry/btAabbUtil2.h"
|
||||
#include "../../gpu_broadphase/host/btSapAabb.h"
|
||||
#include "../../gpu_broadphase/host/btGpuSapBroadphase.h"
|
||||
#include "parallel_primitives/host/btLauncherCL.h"
|
||||
#include "btPgsJacobiSolver.h"
|
||||
#include "../../gpu_sat/host/btRigidBodyCL.h"
|
||||
#include "../../gpu_sat/host/btContact4.h"
|
||||
|
||||
|
||||
btGpuRigidBodyPipeline::btGpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class btGpuNarrowPhase* narrowphase, class btGpuSapBroadphase* broadphaseSap )
|
||||
{
|
||||
@@ -14,6 +20,7 @@ btGpuRigidBodyPipeline::btGpuRigidBodyPipeline(cl_context ctx,cl_device_id devic
|
||||
m_data->m_context = ctx;
|
||||
m_data->m_device = device;
|
||||
m_data->m_queue = q;
|
||||
m_data->m_solver = new btPgsJacobiSolver();
|
||||
|
||||
m_data->m_broadphaseSap = broadphaseSap;
|
||||
m_data->m_narrowphase = narrowphase;
|
||||
@@ -27,6 +34,13 @@ btGpuRigidBodyPipeline::btGpuRigidBodyPipeline(cl_context ctx,cl_device_id devic
|
||||
btAssert(errNum==CL_SUCCESS);
|
||||
clReleaseProgram(prog);
|
||||
}
|
||||
{
|
||||
cl_program prog = btOpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,updateAabbsKernelCL,&errNum,"","opencl/gpu_rigidbody/kernels/updateAabbsKernel.cl");
|
||||
btAssert(errNum==CL_SUCCESS);
|
||||
m_data->m_updateAabbsKernel = btOpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "initializeGpuAabbsFull",&errNum,prog);
|
||||
btAssert(errNum==CL_SUCCESS);
|
||||
clReleaseProgram(prog);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -40,21 +54,103 @@ btGpuRigidBodyPipeline::~btGpuRigidBodyPipeline()
|
||||
|
||||
void btGpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
{
|
||||
btLauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel);
|
||||
//integrateTransformsKernel( __global Body* bodies,const int numNodes, float timeStep, float angularDamping)
|
||||
|
||||
//update worldspace AABBs from local AABB/worldtransform
|
||||
setupGpuAabbsFull();
|
||||
|
||||
//compute overlapping pairs
|
||||
m_data->m_broadphaseSap->calculateOverlappingPairs();
|
||||
|
||||
//compute contact points
|
||||
|
||||
int numPairs = m_data->m_broadphaseSap->getNumOverlap();
|
||||
int numContacts = 0;
|
||||
|
||||
if (numPairs)
|
||||
{
|
||||
cl_mem pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer();
|
||||
cl_mem aabbs = m_data->m_broadphaseSap->getAabbBuffer();
|
||||
int numBodies = m_data->m_narrowphase->getNumBodiesGpu();
|
||||
|
||||
m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbs,numBodies);
|
||||
numContacts = m_data->m_narrowphase->getNumContactsGpu();
|
||||
//if (numContacts)
|
||||
// printf("numContacts = %d\n", numContacts);
|
||||
}
|
||||
|
||||
|
||||
//convert contact points to contact constraints
|
||||
|
||||
//solve constraints
|
||||
|
||||
if (numContacts)
|
||||
{
|
||||
// m_data->m_solver->solveGroup(bodies, inertias,numBodies,contacts,numContacts,0,0,infoGlobal);
|
||||
btAlignedObjectArray<btRigidBodyCL> hostBodies;
|
||||
btOpenCLArray<btRigidBodyCL> gpuBodies(m_data->m_context,m_data->m_queue,0,true);
|
||||
gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumBodiesGpu());
|
||||
gpuBodies.copyToHost(hostBodies);
|
||||
|
||||
btAlignedObjectArray<btInertiaCL> hostInertias;
|
||||
btOpenCLArray<btInertiaCL> gpuInertias(m_data->m_context,m_data->m_queue,0,true);
|
||||
gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumBodiesGpu());
|
||||
gpuInertias.copyToHost(hostInertias);
|
||||
|
||||
btAlignedObjectArray<btContact4> hostContacts;
|
||||
btOpenCLArray<btContact4> gpuContacts(m_data->m_context,m_data->m_queue,0,true);
|
||||
gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu());
|
||||
gpuContacts.copyToHost(hostContacts);
|
||||
|
||||
{
|
||||
m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]);
|
||||
}
|
||||
|
||||
gpuBodies.copyFromHost(hostBodies);
|
||||
}
|
||||
|
||||
integrate(deltaTime);
|
||||
|
||||
}
|
||||
|
||||
void btGpuRigidBodyPipeline::integrate(float timeStep)
|
||||
{
|
||||
//integrate
|
||||
|
||||
btLauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel);
|
||||
launcher.setBuffer(m_data->m_narrowphase->getBodiesGpu());
|
||||
int numBodies = m_data->m_narrowphase->getNumBodiesGpu();
|
||||
launcher.setConst(numBodies);
|
||||
float timeStep = 1./60.f;
|
||||
|
||||
launcher.setConst(timeStep);
|
||||
float angularDamp = 0.99f;
|
||||
launcher.setConst(angularDamp);
|
||||
launcher.launch1D(numBodies);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGpuRigidBodyPipeline::setupGpuAabbsFull()
|
||||
{
|
||||
cl_int ciErrNum=0;
|
||||
|
||||
int numBodies = m_data->m_narrowphase->getNumBodiesGpu();
|
||||
//__kernel void initializeGpuAabbsFull( const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global btAABBCL* plocalShapeAABB, __global btAABBCL* pAABB)
|
||||
btLauncherCL launcher(m_data->m_queue,m_data->m_updateAabbsKernel);
|
||||
launcher.setConst(numBodies);
|
||||
cl_mem bodies = m_data->m_narrowphase->getBodiesGpu();
|
||||
launcher.setBuffer(bodies);
|
||||
cl_mem collidables = m_data->m_narrowphase->getCollidablesGpu();
|
||||
launcher.setBuffer(collidables);
|
||||
cl_mem localAabbs = m_data->m_narrowphase->getAabbBufferGpu();
|
||||
launcher.setBuffer(localAabbs);
|
||||
cl_mem worldAabbs = m_data->m_broadphaseSap->getAabbBuffer();
|
||||
launcher.setBuffer(worldAabbs);
|
||||
launcher.launch1D(numBodies);
|
||||
oclCHECKERROR(ciErrNum, CL_SUCCESS);
|
||||
}
|
||||
|
||||
|
||||
|
||||
cl_mem btGpuRigidBodyPipeline::getBodyBuffer()
|
||||
{
|
||||
return m_data->m_narrowphase->getBodiesGpu();
|
||||
@@ -67,67 +163,6 @@ int btGpuRigidBodyPipeline::getNumBodies() const
|
||||
|
||||
|
||||
|
||||
int btGpuRigidBodyPipeline::registerConvexPolyhedron(btConvexUtility* utilPtr)
|
||||
{
|
||||
/*
|
||||
int collidableIndex = m_narrowphaseAndSolver->allocateCollidable();
|
||||
|
||||
btCollidable& col = m_narrowphaseAndSolver->getCollidableCpu(collidableIndex);
|
||||
col.m_shapeType = CollisionShape::SHAPE_CONVEX_HULL;
|
||||
col.m_shapeIndex = -1;
|
||||
|
||||
|
||||
if (m_narrowphaseAndSolver)
|
||||
{
|
||||
btVector3 localCenter(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 = m_narrowphaseAndSolver->registerConvexHullShape(utilPtr,col);
|
||||
}
|
||||
|
||||
if (col.m_shapeIndex>=0)
|
||||
{
|
||||
btAABBHost aabbMin, aabbMax;
|
||||
btVector3 myAabbMin(1e30f,1e30f,1e30f);
|
||||
btVector3 myAabbMax(-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]);
|
||||
}
|
||||
aabbMin.fx = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
|
||||
aabbMin.fy = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
|
||||
aabbMin.fz= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
|
||||
aabbMin.uw = 0;
|
||||
|
||||
aabbMax.fx = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
|
||||
aabbMax.fy = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
|
||||
aabbMax.fz= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
|
||||
aabbMax.uw = 0;
|
||||
|
||||
m_data->m_localShapeAABBCPU->push_back(aabbMin);
|
||||
m_data->m_localShapeAABBGPU->push_back(aabbMin);
|
||||
|
||||
m_data->m_localShapeAABBCPU->push_back(aabbMax);
|
||||
m_data->m_localShapeAABBGPU->push_back(aabbMax);
|
||||
|
||||
//m_data->m_localShapeAABB->copyFromHostPointer(&aabbMin,1,shapeIndex*2);
|
||||
//m_data->m_localShapeAABB->copyFromHostPointer(&aabbMax,1,shapeIndex*2+1);
|
||||
clFinish(g_cqCommandQue);
|
||||
}
|
||||
|
||||
delete[] eqn;
|
||||
|
||||
|
||||
|
||||
return collidableIndex;
|
||||
*/
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int btGpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex)
|
||||
|
||||
@@ -16,6 +16,8 @@ public:
|
||||
virtual ~btGpuRigidBodyPipeline();
|
||||
|
||||
void stepSimulation(float deltaTime);
|
||||
void integrate(float timeStep);
|
||||
void setupGpuAabbsFull();
|
||||
|
||||
int registerConvexPolyhedron(class btConvexUtility* convex);
|
||||
|
||||
|
||||
@@ -15,6 +15,9 @@ struct btGpuRigidBodyPipelineInternalData
|
||||
cl_command_queue m_queue;
|
||||
|
||||
cl_kernel m_integrateTransformsKernel;
|
||||
cl_kernel m_updateAabbsKernel;
|
||||
|
||||
class btPgsJacobiSolver* m_solver;
|
||||
|
||||
class btGpuSapBroadphase* m_broadphaseSap;
|
||||
|
||||
|
||||
155
opencl/gpu_rigidbody/host/btJacobianEntry.h
Normal file
155
opencl/gpu_rigidbody/host/btJacobianEntry.h
Normal file
@@ -0,0 +1,155 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_JACOBIAN_ENTRY_H
|
||||
#define BT_JACOBIAN_ENTRY_H
|
||||
|
||||
#include "BulletCommon/btMatrix3x3.h"
|
||||
|
||||
|
||||
//notes:
|
||||
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
|
||||
// which makes the btJacobianEntry memory layout 16 bytes
|
||||
// if you only are interested in angular part, just feed massInvA and massInvB zero
|
||||
|
||||
/// Jacobian entry is an abstraction that allows to describe constraints
|
||||
/// it can be used in combination with a constraint solver
|
||||
/// Can be used to relate the effect of an impulse to the constraint error
|
||||
ATTRIBUTE_ALIGNED16(class) btJacobianEntry
|
||||
{
|
||||
public:
|
||||
btJacobianEntry() {};
|
||||
//constraint between two different rigidbodies
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar massInvA,
|
||||
const btVector3& inertiaInvB,
|
||||
const btScalar massInvB)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
|
||||
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& jointAxis,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
{
|
||||
m_aJ= world2A*jointAxis;
|
||||
m_bJ = world2B*-jointAxis;
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& axisInA,
|
||||
const btVector3& axisInB,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
, m_aJ(axisInA)
|
||||
, m_bJ(-axisInB)
|
||||
{
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar massInvA)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ= world2A*(rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
btScalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
|
||||
btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
|
||||
return lin + ang;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
btVector3 lin0 = massInvA * lin ;
|
||||
btVector3 lin1 = massInvB * lin;
|
||||
btVector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
}
|
||||
|
||||
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
|
||||
{
|
||||
btVector3 linrel = linvelA - linvelB;
|
||||
btVector3 angvela = angvelA * m_aJ;
|
||||
btVector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
return rel_vel2 + SIMD_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
btVector3 m_linearJointAxis;
|
||||
btVector3 m_aJ;
|
||||
btVector3 m_bJ;
|
||||
btVector3 m_0MinvJt;
|
||||
btVector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
btScalar m_Adiag;
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_JACOBIAN_ENTRY_H
|
||||
1818
opencl/gpu_rigidbody/host/btPgsJacobiSolver.cpp
Normal file
1818
opencl/gpu_rigidbody/host/btPgsJacobiSolver.cpp
Normal file
File diff suppressed because it is too large
Load Diff
145
opencl/gpu_rigidbody/host/btPgsJacobiSolver.h
Normal file
145
opencl/gpu_rigidbody/host/btPgsJacobiSolver.h
Normal file
@@ -0,0 +1,145 @@
|
||||
#ifndef BT_PGS_JACOBI_SOLVER
|
||||
#define BT_PGS_JACOBI_SOLVER
|
||||
|
||||
|
||||
struct btContact4;
|
||||
struct btContactPoint;
|
||||
|
||||
|
||||
class btDispatcher;
|
||||
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "btConstraintSolver.h"
|
||||
struct btRigidBodyCL;
|
||||
struct btInertiaCL;
|
||||
|
||||
class btPgsJacobiSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
|
||||
btConstraintArray m_tmpSolverContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverNonContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
|
||||
|
||||
btAlignedObjectArray<int> m_orderTmpConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||||
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
|
||||
|
||||
btAlignedObjectArray<int> m_bodyCount;
|
||||
btAlignedObjectArray<int> m_bodyCountCheck;
|
||||
|
||||
btAlignedObjectArray<btVector3> m_deltaLinearVelocities;
|
||||
btAlignedObjectArray<btVector3> m_deltaAngularVelocities;
|
||||
|
||||
bool m_usePgs;
|
||||
void averageVelocities();
|
||||
|
||||
int m_maxOverrideNumSolverIterations;
|
||||
btScalar getContactProcessingThreshold(btContact4* contact)
|
||||
{
|
||||
return 0.02f;
|
||||
}
|
||||
void setupFrictionConstraint( btRigidBodyCL* bodies,btInertiaCL* inertias, btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btContactPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btRigidBodyCL* colObj0,btRigidBodyCL* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
void setupRollingFrictionConstraint(btRigidBodyCL* bodies,btInertiaCL* inertias, btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btContactPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btRigidBodyCL* colObj0,btRigidBodyCL* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
btSolverConstraint& addFrictionConstraint(btRigidBodyCL* bodies,btInertiaCL* inertias,const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btRigidBodyCL* colObj0,btRigidBodyCL* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
btSolverConstraint& addRollingFrictionConstraint(btRigidBodyCL* bodies,btInertiaCL* inertias,const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btRigidBodyCL* colObj0,btRigidBodyCL* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
|
||||
|
||||
|
||||
void setupContactConstraint(btRigidBodyCL* bodies, btInertiaCL* inertias,
|
||||
btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btContactPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2);
|
||||
|
||||
void setFrictionConstraintImpulse( btRigidBodyCL* bodies, btInertiaCL* inertias,btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
btContactPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
|
||||
|
||||
void convertContact(btRigidBodyCL* bodies, btInertiaCL* inertias,btContact4* manifold,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
void resolveSplitPenetrationSIMD(
|
||||
btSolverBody& bodyA,btSolverBody& bodyB,
|
||||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSplitPenetrationImpulseCacheFriendly(
|
||||
btSolverBody& bodyA,btSolverBody& bodyB,
|
||||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
//internal method
|
||||
int getOrInitSolverBody(int bodyIndex, btRigidBodyCL* bodies,btInertiaCL* inertias);
|
||||
void initSolverBody(int bodyIndex, btSolverBody* solverBody, btRigidBodyCL* collisionObject);
|
||||
|
||||
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
protected:
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btRigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlyIterations(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
btScalar solveSingleIteration(int iteration, btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btRigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btPgsJacobiSolver();
|
||||
virtual ~btPgsJacobiSolver();
|
||||
|
||||
void solveContacts(int numBodies, btRigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, btContact4* contacts);
|
||||
|
||||
btScalar solveGroup(btRigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset();
|
||||
|
||||
unsigned long btRand2();
|
||||
|
||||
int btRandInt2 (int n);
|
||||
|
||||
void setRandSeed(unsigned long seed)
|
||||
{
|
||||
m_btSeed2 = seed;
|
||||
}
|
||||
unsigned long getRandSeed() const
|
||||
{
|
||||
return m_btSeed2;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_PGS_JACOBI_SOLVER
|
||||
|
||||
594
opencl/gpu_rigidbody/host/btRigidBody.h
Normal file
594
opencl/gpu_rigidbody/host/btRigidBody.h
Normal file
@@ -0,0 +1,594 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_RIGIDBODY_H
|
||||
#define BT_RIGIDBODY_H
|
||||
|
||||
#include "BulletCommon/btAlignedObjectArray.h"
|
||||
#include "BulletCommon/btTransform.h"
|
||||
#include "btBroadphaseProxy.h"
|
||||
#include "btCollisionObject.h"
|
||||
|
||||
class btCollisionShape;
|
||||
class btMotionState;
|
||||
class btTypedConstraint;
|
||||
|
||||
|
||||
extern btScalar gDeactivationTime;
|
||||
extern bool gDisableDeactivation;
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btRigidBodyData btRigidBodyDoubleData
|
||||
#define btRigidBodyDataName "btRigidBodyDoubleData"
|
||||
#else
|
||||
#define btRigidBodyData btRigidBodyFloatData
|
||||
#define btRigidBodyDataName "btRigidBodyFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
enum btRigidBodyFlags
|
||||
{
|
||||
BT_DISABLE_WORLD_GRAVITY = 1,
|
||||
///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
|
||||
///So generally it is best to not enable it.
|
||||
///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use
|
||||
BT_ENABLE_GYROPSCOPIC_FORCE = 2
|
||||
};
|
||||
|
||||
|
||||
///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
|
||||
///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
|
||||
///There are 3 types of rigid bodies:
|
||||
///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
|
||||
///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
|
||||
///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
|
||||
///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
|
||||
///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
|
||||
class btRigidBody : public btCollisionObject
|
||||
{
|
||||
|
||||
btMatrix3x3 m_invInertiaTensorWorld;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
btScalar m_inverseMass;
|
||||
btVector3 m_linearFactor;
|
||||
|
||||
btVector3 m_gravity;
|
||||
btVector3 m_gravity_acceleration;
|
||||
btVector3 m_invInertiaLocal;
|
||||
btVector3 m_totalForce;
|
||||
btVector3 m_totalTorque;
|
||||
|
||||
btScalar m_linearDamping;
|
||||
btScalar m_angularDamping;
|
||||
|
||||
bool m_additionalDamping;
|
||||
btScalar m_additionalDampingFactor;
|
||||
btScalar m_additionalLinearDampingThresholdSqr;
|
||||
btScalar m_additionalAngularDampingThresholdSqr;
|
||||
btScalar m_additionalAngularDampingFactor;
|
||||
|
||||
|
||||
btScalar m_linearSleepingThreshold;
|
||||
btScalar m_angularSleepingThreshold;
|
||||
|
||||
//m_optionalMotionState allows to automatic synchronize the world transform for active objects
|
||||
btMotionState* m_optionalMotionState;
|
||||
|
||||
//keep track of typed constraints referencing this rigid body
|
||||
btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
|
||||
|
||||
int m_rigidbodyFlags;
|
||||
|
||||
int m_debugBodyId;
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
btVector3 m_angularFactor;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
|
||||
///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
|
||||
///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
|
||||
///You can use the motion state to synchronize the world transform between physics and graphics objects.
|
||||
///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
|
||||
///m_startWorldTransform is only used when you don't provide a motion state.
|
||||
struct btRigidBodyConstructionInfo
|
||||
{
|
||||
btScalar m_mass;
|
||||
|
||||
///When a motionState is provided, the rigid body will initialize its world transform from the motion state
|
||||
///In this case, m_startWorldTransform is ignored.
|
||||
btMotionState* m_motionState;
|
||||
btTransform m_startWorldTransform;
|
||||
|
||||
btCollisionShape* m_collisionShape;
|
||||
btVector3 m_localInertia;
|
||||
btScalar m_linearDamping;
|
||||
btScalar m_angularDamping;
|
||||
|
||||
///best simulation results when friction is non-zero
|
||||
btScalar m_friction;
|
||||
///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
|
||||
///See Bullet/Demos/RollingFrictionDemo for usage
|
||||
btScalar m_rollingFriction;
|
||||
///best simulation results using zero restitution.
|
||||
btScalar m_restitution;
|
||||
|
||||
btScalar m_linearSleepingThreshold;
|
||||
btScalar m_angularSleepingThreshold;
|
||||
|
||||
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
|
||||
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
|
||||
bool m_additionalDamping;
|
||||
btScalar m_additionalDampingFactor;
|
||||
btScalar m_additionalLinearDampingThresholdSqr;
|
||||
btScalar m_additionalAngularDampingThresholdSqr;
|
||||
btScalar m_additionalAngularDampingFactor;
|
||||
|
||||
btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
|
||||
m_mass(mass),
|
||||
m_motionState(motionState),
|
||||
m_collisionShape(collisionShape),
|
||||
m_localInertia(localInertia),
|
||||
m_linearDamping(btScalar(0.)),
|
||||
m_angularDamping(btScalar(0.)),
|
||||
m_friction(btScalar(0.5)),
|
||||
m_rollingFriction(btScalar(0)),
|
||||
m_restitution(btScalar(0.)),
|
||||
m_linearSleepingThreshold(btScalar(0.8)),
|
||||
m_angularSleepingThreshold(btScalar(1.f)),
|
||||
m_additionalDamping(false),
|
||||
m_additionalDampingFactor(btScalar(0.005)),
|
||||
m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
|
||||
m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
|
||||
m_additionalAngularDampingFactor(btScalar(0.01))
|
||||
{
|
||||
m_startWorldTransform.setIdentity();
|
||||
}
|
||||
};
|
||||
|
||||
///btRigidBody constructor using construction info
|
||||
btRigidBody( const btRigidBodyConstructionInfo& constructionInfo);
|
||||
|
||||
///btRigidBody constructor for backwards compatibility.
|
||||
///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
|
||||
btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
|
||||
|
||||
|
||||
virtual ~btRigidBody()
|
||||
{
|
||||
//No constraints should point to this rigidbody
|
||||
//Remove constraints from the dynamics world before you delete the related rigidbodies.
|
||||
btAssert(m_constraintRefs.size()==0);
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
///setupRigidBody is only used internally by the constructor
|
||||
void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
|
||||
|
||||
public:
|
||||
|
||||
void proceedToTransform(const btTransform& newTrans);
|
||||
|
||||
///to keep collision detection and dynamics separate we don't store a rigidbody pointer
|
||||
///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
|
||||
static const btRigidBody* upcast(const btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
|
||||
return (const btRigidBody*)colObj;
|
||||
return 0;
|
||||
}
|
||||
static btRigidBody* upcast(btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
|
||||
return (btRigidBody*)colObj;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// continuous collision detection needs prediction
|
||||
void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
|
||||
|
||||
void saveKinematicState(btScalar step);
|
||||
|
||||
void applyGravity();
|
||||
|
||||
void setGravity(const btVector3& acceleration);
|
||||
|
||||
const btVector3& getGravity() const
|
||||
{
|
||||
return m_gravity_acceleration;
|
||||
}
|
||||
|
||||
void setDamping(btScalar lin_damping, btScalar ang_damping);
|
||||
|
||||
btScalar getLinearDamping() const
|
||||
{
|
||||
return m_linearDamping;
|
||||
}
|
||||
|
||||
btScalar getAngularDamping() const
|
||||
{
|
||||
return m_angularDamping;
|
||||
}
|
||||
|
||||
btScalar getLinearSleepingThreshold() const
|
||||
{
|
||||
return m_linearSleepingThreshold;
|
||||
}
|
||||
|
||||
btScalar getAngularSleepingThreshold() const
|
||||
{
|
||||
return m_angularSleepingThreshold;
|
||||
}
|
||||
|
||||
void applyDamping(btScalar timeStep);
|
||||
|
||||
SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
void setMassProps(btScalar mass, const btVector3& inertia);
|
||||
|
||||
const btVector3& getLinearFactor() const
|
||||
{
|
||||
return m_linearFactor;
|
||||
}
|
||||
void setLinearFactor(const btVector3& linearFactor)
|
||||
{
|
||||
m_linearFactor = linearFactor;
|
||||
}
|
||||
btScalar getInvMass() const { return m_inverseMass; }
|
||||
const btMatrix3x3& getInvInertiaTensorWorld() const {
|
||||
return m_invInertiaTensorWorld;
|
||||
}
|
||||
|
||||
void integrateVelocities(btScalar step);
|
||||
|
||||
void setCenterOfMassTransform(const btTransform& xform);
|
||||
|
||||
void applyCentralForce(const btVector3& force)
|
||||
{
|
||||
m_totalForce += force*m_linearFactor;
|
||||
}
|
||||
|
||||
const btVector3& getTotalForce() const
|
||||
{
|
||||
return m_totalForce;
|
||||
};
|
||||
|
||||
const btVector3& getTotalTorque() const
|
||||
{
|
||||
return m_totalTorque;
|
||||
};
|
||||
|
||||
const btVector3& getInvInertiaDiagLocal() const
|
||||
{
|
||||
return m_invInertiaLocal;
|
||||
};
|
||||
|
||||
void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
|
||||
{
|
||||
m_invInertiaLocal = diagInvInertia;
|
||||
}
|
||||
|
||||
void setSleepingThresholds(btScalar linear,btScalar angular)
|
||||
{
|
||||
m_linearSleepingThreshold = linear;
|
||||
m_angularSleepingThreshold = angular;
|
||||
}
|
||||
|
||||
void applyTorque(const btVector3& torque)
|
||||
{
|
||||
m_totalTorque += torque*m_angularFactor;
|
||||
}
|
||||
|
||||
void applyForce(const btVector3& force, const btVector3& rel_pos)
|
||||
{
|
||||
applyCentralForce(force);
|
||||
applyTorque(rel_pos.cross(force*m_linearFactor));
|
||||
}
|
||||
|
||||
void applyCentralImpulse(const btVector3& impulse)
|
||||
{
|
||||
m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
|
||||
}
|
||||
|
||||
void applyTorqueImpulse(const btVector3& torque)
|
||||
{
|
||||
m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
|
||||
}
|
||||
|
||||
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
|
||||
{
|
||||
if (m_inverseMass != btScalar(0.))
|
||||
{
|
||||
applyCentralImpulse(impulse);
|
||||
if (m_angularFactor)
|
||||
{
|
||||
applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void clearForces()
|
||||
{
|
||||
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
|
||||
m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
|
||||
}
|
||||
|
||||
void updateInertiaTensor();
|
||||
|
||||
const btVector3& getCenterOfMassPosition() const {
|
||||
return m_worldTransform.getOrigin();
|
||||
}
|
||||
btQuaternion getOrientation() const;
|
||||
|
||||
const btTransform& getCenterOfMassTransform() const {
|
||||
return m_worldTransform;
|
||||
}
|
||||
const btVector3& getLinearVelocity() const {
|
||||
return m_linearVelocity;
|
||||
}
|
||||
const btVector3& getAngularVelocity() const {
|
||||
return m_angularVelocity;
|
||||
}
|
||||
|
||||
|
||||
inline void setLinearVelocity(const btVector3& lin_vel)
|
||||
{
|
||||
m_linearVelocity = lin_vel;
|
||||
}
|
||||
|
||||
inline void setAngularVelocity(const btVector3& ang_vel)
|
||||
{
|
||||
m_angularVelocity = ang_vel;
|
||||
}
|
||||
|
||||
btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
|
||||
{
|
||||
//we also calculate lin/ang velocity for kinematic objects
|
||||
return m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
|
||||
//for kinematic objects, we could also use use:
|
||||
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
|
||||
}
|
||||
|
||||
void translate(const btVector3& v)
|
||||
{
|
||||
m_worldTransform.getOrigin() += v;
|
||||
}
|
||||
|
||||
|
||||
void getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
|
||||
{
|
||||
btVector3 r0 = pos - getCenterOfMassPosition();
|
||||
|
||||
btVector3 c0 = (r0).cross(normal);
|
||||
|
||||
btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
|
||||
|
||||
return m_inverseMass + normal.dot(vec);
|
||||
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
|
||||
{
|
||||
btVector3 vec = axis * getInvInertiaTensorWorld();
|
||||
return axis.dot(vec);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
|
||||
{
|
||||
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
|
||||
return;
|
||||
|
||||
if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
|
||||
(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
|
||||
{
|
||||
m_deactivationTime += timeStep;
|
||||
} else
|
||||
{
|
||||
m_deactivationTime=btScalar(0.);
|
||||
setActivationState(0);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE bool wantsSleeping()
|
||||
{
|
||||
|
||||
if (getActivationState() == DISABLE_DEACTIVATION)
|
||||
return false;
|
||||
|
||||
//disable deactivation
|
||||
if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
|
||||
return false;
|
||||
|
||||
if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
|
||||
return true;
|
||||
|
||||
if (m_deactivationTime> gDeactivationTime)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
const btBroadphaseProxy* getBroadphaseProxy() const
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
btBroadphaseProxy* getBroadphaseProxy()
|
||||
{
|
||||
return m_broadphaseHandle;
|
||||
}
|
||||
void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
|
||||
{
|
||||
m_broadphaseHandle = broadphaseProxy;
|
||||
}
|
||||
|
||||
//btMotionState allows to automatic synchronize the world transform for active objects
|
||||
btMotionState* getMotionState()
|
||||
{
|
||||
return m_optionalMotionState;
|
||||
}
|
||||
const btMotionState* getMotionState() const
|
||||
{
|
||||
return m_optionalMotionState;
|
||||
}
|
||||
void setMotionState(btMotionState* motionState)
|
||||
{
|
||||
m_optionalMotionState = motionState;
|
||||
if (m_optionalMotionState)
|
||||
motionState->getWorldTransform(m_worldTransform);
|
||||
}
|
||||
|
||||
//for experimental overriding of friction/contact solver func
|
||||
int m_contactSolverType;
|
||||
int m_frictionSolverType;
|
||||
|
||||
void setAngularFactor(const btVector3& angFac)
|
||||
{
|
||||
m_angularFactor = angFac;
|
||||
}
|
||||
|
||||
void setAngularFactor(btScalar angFac)
|
||||
{
|
||||
m_angularFactor.setValue(angFac,angFac,angFac);
|
||||
}
|
||||
const btVector3& getAngularFactor() const
|
||||
{
|
||||
return m_angularFactor;
|
||||
}
|
||||
|
||||
//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
|
||||
bool isInWorld() const
|
||||
{
|
||||
return (getBroadphaseProxy() != 0);
|
||||
}
|
||||
|
||||
virtual bool checkCollideWithOverride(const btCollisionObject* co) const;
|
||||
|
||||
void addConstraintRef(btTypedConstraint* c);
|
||||
void removeConstraintRef(btTypedConstraint* c);
|
||||
|
||||
btTypedConstraint* getConstraintRef(int index)
|
||||
{
|
||||
return m_constraintRefs[index];
|
||||
}
|
||||
|
||||
int getNumConstraintRefs() const
|
||||
{
|
||||
return m_constraintRefs.size();
|
||||
}
|
||||
|
||||
void setFlags(int flags)
|
||||
{
|
||||
m_rigidbodyFlags = flags;
|
||||
}
|
||||
|
||||
int getFlags() const
|
||||
{
|
||||
return m_rigidbodyFlags;
|
||||
}
|
||||
|
||||
btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
|
||||
|
||||
///////////////////////////////////////////////
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
|
||||
|
||||
virtual void serializeSingleObject(class btSerializer* serializer) const;
|
||||
|
||||
};
|
||||
|
||||
//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btRigidBodyFloatData
|
||||
{
|
||||
btCollisionObjectFloatData m_collisionObjectData;
|
||||
btMatrix3x3FloatData m_invInertiaTensorWorld;
|
||||
btVector3FloatData m_linearVelocity;
|
||||
btVector3FloatData m_angularVelocity;
|
||||
btVector3FloatData m_angularFactor;
|
||||
btVector3FloatData m_linearFactor;
|
||||
btVector3FloatData m_gravity;
|
||||
btVector3FloatData m_gravity_acceleration;
|
||||
btVector3FloatData m_invInertiaLocal;
|
||||
btVector3FloatData m_totalForce;
|
||||
btVector3FloatData m_totalTorque;
|
||||
float m_inverseMass;
|
||||
float m_linearDamping;
|
||||
float m_angularDamping;
|
||||
float m_additionalDampingFactor;
|
||||
float m_additionalLinearDampingThresholdSqr;
|
||||
float m_additionalAngularDampingThresholdSqr;
|
||||
float m_additionalAngularDampingFactor;
|
||||
float m_linearSleepingThreshold;
|
||||
float m_angularSleepingThreshold;
|
||||
int m_additionalDamping;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btRigidBodyDoubleData
|
||||
{
|
||||
btCollisionObjectDoubleData m_collisionObjectData;
|
||||
btMatrix3x3DoubleData m_invInertiaTensorWorld;
|
||||
btVector3DoubleData m_linearVelocity;
|
||||
btVector3DoubleData m_angularVelocity;
|
||||
btVector3DoubleData m_angularFactor;
|
||||
btVector3DoubleData m_linearFactor;
|
||||
btVector3DoubleData m_gravity;
|
||||
btVector3DoubleData m_gravity_acceleration;
|
||||
btVector3DoubleData m_invInertiaLocal;
|
||||
btVector3DoubleData m_totalForce;
|
||||
btVector3DoubleData m_totalTorque;
|
||||
double m_inverseMass;
|
||||
double m_linearDamping;
|
||||
double m_angularDamping;
|
||||
double m_additionalDampingFactor;
|
||||
double m_additionalLinearDampingThresholdSqr;
|
||||
double m_additionalAngularDampingThresholdSqr;
|
||||
double m_additionalAngularDampingFactor;
|
||||
double m_linearSleepingThreshold;
|
||||
double m_angularSleepingThreshold;
|
||||
int m_additionalDamping;
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //BT_RIGIDBODY_H
|
||||
|
||||
299
opencl/gpu_rigidbody/host/btSolverBody.h
Normal file
299
opencl/gpu_rigidbody/host/btSolverBody.h
Normal file
@@ -0,0 +1,299 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_SOLVER_BODY_H
|
||||
#define BT_SOLVER_BODY_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "BulletCommon/btVector3.h"
|
||||
#include "BulletCommon/btMatrix3x3.h"
|
||||
|
||||
#include "BulletCommon/btAlignedAllocator.h"
|
||||
#include "BulletCommon/btTransformUtil.h"
|
||||
|
||||
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
|
||||
#ifdef BT_USE_SSE
|
||||
#define USE_SIMD 1
|
||||
#endif //
|
||||
|
||||
|
||||
#ifdef USE_SIMD
|
||||
|
||||
struct btSimdScalar
|
||||
{
|
||||
SIMD_FORCE_INLINE btSimdScalar()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(float fl)
|
||||
:m_vec128 (_mm_set1_ps(fl))
|
||||
{
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
|
||||
:m_vec128(v128)
|
||||
{
|
||||
}
|
||||
union
|
||||
{
|
||||
__m128 m_vec128;
|
||||
float m_floats[4];
|
||||
int m_ints[4];
|
||||
btScalar m_unusedPadding;
|
||||
};
|
||||
SIMD_FORCE_INLINE __m128 get128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE const __m128 get128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void set128(__m128 v128)
|
||||
{
|
||||
m_vec128 = v128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator __m128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
SIMD_FORCE_INLINE operator const __m128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator float() const
|
||||
{
|
||||
return m_floats[0];
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
///@brief Return the elementwise product of two btSimdScalar
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
{
|
||||
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
|
||||
}
|
||||
|
||||
///@brief Return the elementwise product of two btSimdScalar
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
{
|
||||
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
#define btSimdScalar btScalar
|
||||
#endif
|
||||
|
||||
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
ATTRIBUTE_ALIGNED64 (struct) btSolverBody
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
btTransform m_worldTransform;
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btVector3 m_angularFactor;
|
||||
btVector3 m_linearFactor;
|
||||
btVector3 m_invMass;
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
|
||||
union
|
||||
{
|
||||
void* m_originalBody;
|
||||
int m_originalBodyIndex;
|
||||
};
|
||||
|
||||
|
||||
void setWorldTransform(const btTransform& worldTransform)
|
||||
{
|
||||
m_worldTransform = worldTransform;
|
||||
}
|
||||
|
||||
const btTransform& getWorldTransform() const
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
|
||||
else
|
||||
velocity.setValue(0,0,0);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
angVel =m_angularVelocity+m_deltaAngularVelocity;
|
||||
else
|
||||
angVel.setValue(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
const btVector3& getDeltaLinearVelocity() const
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getDeltaAngularVelocity() const
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getPushVelocity() const
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getTurnVelocity() const
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////
|
||||
///some internal methods, don't use them
|
||||
|
||||
btVector3& internalGetDeltaLinearVelocity()
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
btVector3& internalGetDeltaAngularVelocity()
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& internalGetAngularFactor() const
|
||||
{
|
||||
return m_angularFactor;
|
||||
}
|
||||
|
||||
const btVector3& internalGetInvMass() const
|
||||
{
|
||||
return m_invMass;
|
||||
}
|
||||
|
||||
void internalSetInvMass(const btVector3& invMass)
|
||||
{
|
||||
m_invMass = invMass;
|
||||
}
|
||||
|
||||
btVector3& internalGetPushVelocity()
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
btVector3& internalGetTurnVelocity()
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
|
||||
{
|
||||
angVel = m_angularVelocity+m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_linearVelocity +=m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
|
||||
{
|
||||
(void) timeStep;
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_linearVelocity += m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
btTransform newTransform;
|
||||
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
|
||||
{
|
||||
// btQuaternion orn = m_worldTransform.getRotation();
|
||||
btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
|
||||
m_worldTransform = newTransform;
|
||||
}
|
||||
//m_worldTransform.setRotation(orn);
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
|
||||
|
||||
79
opencl/gpu_rigidbody/host/btSolverConstraint.h
Normal file
79
opencl/gpu_rigidbody/host/btSolverConstraint.h
Normal file
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_SOLVER_CONSTRAINT_H
|
||||
#define BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "BulletCommon/btVector3.h"
|
||||
#include "BulletCommon/btMatrix3x3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "BulletCommon/btAlignedObjectArray.h"
|
||||
|
||||
//#define NO_FRICTION_TANGENTIALS 1
|
||||
#include "btSolverBody.h"
|
||||
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal;
|
||||
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
//btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_rhsPenetration;
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
int m_solverBodyIdA;
|
||||
int m_solverBodyIdB;
|
||||
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
|
||||
|
||||
|
||||
#endif //BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
482
opencl/gpu_rigidbody/host/btTypedConstraint.h
Normal file
482
opencl/gpu_rigidbody/host/btTypedConstraint.h
Normal file
@@ -0,0 +1,482 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2010 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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_TYPED_CONSTRAINT_H
|
||||
#define BT_TYPED_CONSTRAINT_H
|
||||
|
||||
|
||||
#include "BulletCommon/btScalar.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "btRigidBody.h"
|
||||
|
||||
class btSerializer;
|
||||
|
||||
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
|
||||
enum btTypedConstraintType
|
||||
{
|
||||
POINT2POINT_CONSTRAINT_TYPE=3,
|
||||
HINGE_CONSTRAINT_TYPE,
|
||||
CONETWIST_CONSTRAINT_TYPE,
|
||||
D6_CONSTRAINT_TYPE,
|
||||
SLIDER_CONSTRAINT_TYPE,
|
||||
CONTACT_CONSTRAINT_TYPE,
|
||||
D6_SPRING_CONSTRAINT_TYPE,
|
||||
GEAR_CONSTRAINT_TYPE,
|
||||
MAX_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
|
||||
enum btConstraintParams
|
||||
{
|
||||
BT_CONSTRAINT_ERP=1,
|
||||
BT_CONSTRAINT_STOP_ERP,
|
||||
BT_CONSTRAINT_CFM,
|
||||
BT_CONSTRAINT_STOP_CFM
|
||||
};
|
||||
|
||||
#if 1
|
||||
#define btAssertConstrParams(_par) btAssert(_par)
|
||||
#else
|
||||
#define btAssertConstrParams(_par)
|
||||
#endif
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
|
||||
{
|
||||
btVector3 m_appliedForceBodyA;
|
||||
btVector3 m_appliedTorqueBodyA;
|
||||
btVector3 m_appliedForceBodyB;
|
||||
btVector3 m_appliedTorqueBodyB;
|
||||
};
|
||||
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
|
||||
{
|
||||
int m_userConstraintType;
|
||||
|
||||
union
|
||||
{
|
||||
int m_userConstraintId;
|
||||
void* m_userConstraintPtr;
|
||||
};
|
||||
|
||||
btScalar m_breakingImpulseThreshold;
|
||||
bool m_isEnabled;
|
||||
bool m_needsFeedback;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
|
||||
btTypedConstraint& operator=(btTypedConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
btRigidBody& m_rbA;
|
||||
btRigidBody& m_rbB;
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_dbgDrawSize;
|
||||
btJointFeedback* m_jointFeedback;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
virtual ~btTypedConstraint() {};
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
|
||||
|
||||
struct btConstraintInfo1 {
|
||||
int m_numConstraintRows,nub;
|
||||
};
|
||||
|
||||
static btRigidBody& getFixedBody();
|
||||
|
||||
struct btConstraintInfo2 {
|
||||
// integrator parameters: frames per second (1/stepsize), default error
|
||||
// reduction parameter (0..1).
|
||||
btScalar fps,erp;
|
||||
|
||||
// for the first and second body, pointers to two (linear and angular)
|
||||
// n*3 jacobian sub matrices, stored by rows. these matrices will have
|
||||
// been initialized to 0 on entry. if the second body is zero then the
|
||||
// J2xx pointers may be 0.
|
||||
btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
|
||||
|
||||
// elements to jump from one row to the next in J's
|
||||
int rowskip;
|
||||
|
||||
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
|
||||
// "constraint force mixing" vector. c is set to zero on entry, cfm is
|
||||
// set to a constant value (typically very small or zero) value on entry.
|
||||
btScalar *m_constraintError,*cfm;
|
||||
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
btScalar *m_lowerLimit,*m_upperLimit;
|
||||
|
||||
// findex vector for variables. see the LCP solver interface for a
|
||||
// description of what this does. this is set to -1 on entry.
|
||||
// note that the returned indexes are relative to the first index of
|
||||
// the constraint.
|
||||
int *findex;
|
||||
// number of solver iterations
|
||||
int m_numIterations;
|
||||
|
||||
//damping of the velocity
|
||||
btScalar m_damping;
|
||||
};
|
||||
|
||||
int getOverrideNumSolverIterations() const
|
||||
{
|
||||
return m_overrideNumSolverIterations;
|
||||
}
|
||||
|
||||
///override the number of constraint solver iterations used to solve this constraint
|
||||
///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
|
||||
void setOverrideNumSolverIterations(int overideNumIterations)
|
||||
{
|
||||
m_overrideNumSolverIterations = overideNumIterations;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void buildJacobian() {};
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
|
||||
{
|
||||
(void)ca;
|
||||
(void)solverBodyA;
|
||||
(void)solverBodyB;
|
||||
(void)timeStep;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo1 (btConstraintInfo1* info)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
void internalSetAppliedImpulse(btScalar appliedImpulse)
|
||||
{
|
||||
m_appliedImpulse = appliedImpulse;
|
||||
}
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
btScalar internalGetAppliedImpulse()
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar getBreakingImpulseThreshold() const
|
||||
{
|
||||
return m_breakingImpulseThreshold;
|
||||
}
|
||||
|
||||
void setBreakingImpulseThreshold(btScalar threshold)
|
||||
{
|
||||
m_breakingImpulseThreshold = threshold;
|
||||
}
|
||||
|
||||
bool isEnabled() const
|
||||
{
|
||||
return m_isEnabled;
|
||||
}
|
||||
|
||||
void setEnabled(bool enabled)
|
||||
{
|
||||
m_isEnabled=enabled;
|
||||
}
|
||||
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {};
|
||||
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
btRigidBody& getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
int getUserConstraintType() const
|
||||
{
|
||||
return m_userConstraintType ;
|
||||
}
|
||||
|
||||
void setUserConstraintType(int userConstraintType)
|
||||
{
|
||||
m_userConstraintType = userConstraintType;
|
||||
};
|
||||
|
||||
void setUserConstraintId(int uid)
|
||||
{
|
||||
m_userConstraintId = uid;
|
||||
}
|
||||
|
||||
int getUserConstraintId() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
void setUserConstraintPtr(void* ptr)
|
||||
{
|
||||
m_userConstraintPtr = ptr;
|
||||
}
|
||||
|
||||
void* getUserConstraintPtr()
|
||||
{
|
||||
return m_userConstraintPtr;
|
||||
}
|
||||
|
||||
void setJointFeedback(btJointFeedback* jointFeedback)
|
||||
{
|
||||
m_jointFeedback = jointFeedback;
|
||||
}
|
||||
|
||||
const btJointFeedback* getJointFeedback() const
|
||||
{
|
||||
return m_jointFeedback;
|
||||
}
|
||||
|
||||
btJointFeedback* getJointFeedback()
|
||||
{
|
||||
return m_jointFeedback;
|
||||
}
|
||||
|
||||
|
||||
int getUid() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
bool needsFeedback() const
|
||||
{
|
||||
return m_needsFeedback;
|
||||
}
|
||||
|
||||
///enableFeedback will allow to read the applied linear and angular impulse
|
||||
///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
|
||||
void enableFeedback(bool needsFeedback)
|
||||
{
|
||||
m_needsFeedback = needsFeedback;
|
||||
}
|
||||
|
||||
///getAppliedImpulse is an estimated total applied impulse.
|
||||
///This feedback could be used to determine breaking constraints or playing sounds.
|
||||
btScalar getAppliedImpulse() const
|
||||
{
|
||||
btAssert(m_needsFeedback);
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
btTypedConstraintType getConstraintType () const
|
||||
{
|
||||
return btTypedConstraintType(m_objectType);
|
||||
}
|
||||
|
||||
void setDbgDrawSize(btScalar dbgDrawSize)
|
||||
{
|
||||
m_dbgDrawSize = dbgDrawSize;
|
||||
}
|
||||
btScalar getDbgDrawSize()
|
||||
{
|
||||
return m_dbgDrawSize;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1) = 0;
|
||||
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const = 0;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
};
|
||||
|
||||
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
|
||||
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
|
||||
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
|
||||
{
|
||||
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
else if(angleInRadians < angleLowerLimitInRadians)
|
||||
{
|
||||
btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
|
||||
btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
|
||||
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
|
||||
}
|
||||
else if(angleInRadians > angleUpperLimitInRadians)
|
||||
{
|
||||
btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
|
||||
btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
|
||||
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
|
||||
}
|
||||
else
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
}
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btTypedConstraintData
|
||||
{
|
||||
btRigidBodyData *m_rbA;
|
||||
btRigidBodyData *m_rbB;
|
||||
char *m_name;
|
||||
|
||||
int m_objectType;
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
int m_needsFeedback;
|
||||
|
||||
float m_appliedImpulse;
|
||||
float m_dbgDrawSize;
|
||||
|
||||
int m_disableCollisionsBetweenLinkedBodies;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
float m_breakingImpulseThreshold;
|
||||
int m_isEnabled;
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btTypedConstraintData);
|
||||
}
|
||||
|
||||
|
||||
|
||||
class btAngularLimit
|
||||
{
|
||||
private:
|
||||
btScalar
|
||||
m_center,
|
||||
m_halfRange,
|
||||
m_softness,
|
||||
m_biasFactor,
|
||||
m_relaxationFactor,
|
||||
m_correction,
|
||||
m_sign;
|
||||
|
||||
bool
|
||||
m_solveLimit;
|
||||
|
||||
public:
|
||||
/// Default constructor initializes limit as inactive, allowing free constraint movement
|
||||
btAngularLimit()
|
||||
:m_center(0.0f),
|
||||
m_halfRange(-1.0f),
|
||||
m_softness(0.9f),
|
||||
m_biasFactor(0.3f),
|
||||
m_relaxationFactor(1.0f),
|
||||
m_correction(0.0f),
|
||||
m_sign(0.0f),
|
||||
m_solveLimit(false)
|
||||
{}
|
||||
|
||||
/// Sets all limit's parameters.
|
||||
/// When low > high limit becomes inactive.
|
||||
/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
|
||||
void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
|
||||
|
||||
/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
|
||||
/// correction is calculated.
|
||||
void test(const btScalar angle);
|
||||
|
||||
/// Returns limit's softness
|
||||
inline btScalar getSoftness() const
|
||||
{
|
||||
return m_softness;
|
||||
}
|
||||
|
||||
/// Returns limit's bias factor
|
||||
inline btScalar getBiasFactor() const
|
||||
{
|
||||
return m_biasFactor;
|
||||
}
|
||||
|
||||
/// Returns limit's relaxation factor
|
||||
inline btScalar getRelaxationFactor() const
|
||||
{
|
||||
return m_relaxationFactor;
|
||||
}
|
||||
|
||||
/// Returns correction value evaluated when test() was invoked
|
||||
inline btScalar getCorrection() const
|
||||
{
|
||||
return m_correction;
|
||||
}
|
||||
|
||||
/// Returns sign value evaluated when test() was invoked
|
||||
inline btScalar getSign() const
|
||||
{
|
||||
return m_sign;
|
||||
}
|
||||
|
||||
/// Gives half of the distance between min and max limit angle
|
||||
inline btScalar getHalfRange() const
|
||||
{
|
||||
return m_halfRange;
|
||||
}
|
||||
|
||||
/// Returns true when the last test() invocation recognized limit violation
|
||||
inline bool isLimit() const
|
||||
{
|
||||
return m_solveLimit;
|
||||
}
|
||||
|
||||
/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
|
||||
/// returned is modified so it equals to the limit closest to given angle.
|
||||
void fit(btScalar& angle) const;
|
||||
|
||||
/// Returns correction value multiplied by sign value
|
||||
btScalar getError() const;
|
||||
|
||||
btScalar getLow() const;
|
||||
|
||||
btScalar getHigh() const;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //BT_TYPED_CONSTRAINT_H
|
||||
@@ -46,9 +46,9 @@ __kernel void
|
||||
{
|
||||
int nodeID = get_global_id(0);
|
||||
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
|
||||
if( nodeID < numNodes )
|
||||
if( nodeID < numNodes && (bodies[nodeID].m_invMass != 0.f))
|
||||
{
|
||||
if (1)
|
||||
//angular velocity
|
||||
{
|
||||
float4 axis;
|
||||
//add some hardcoded angular damping
|
||||
@@ -82,8 +82,12 @@ __kernel void
|
||||
bodies[nodeID].m_quat=predictedOrn;
|
||||
}
|
||||
|
||||
//linear velocity
|
||||
//linear velocity
|
||||
bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
|
||||
|
||||
//apply gravity
|
||||
float4 gravityAcceleration = (float4)(0.f,-9.8f,0.f,0.f);
|
||||
bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -48,9 +48,9 @@ static const char* integrateKernelCL= \
|
||||
"{\n"
|
||||
" int nodeID = get_global_id(0);\n"
|
||||
" float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n"
|
||||
" if( nodeID < numNodes )\n"
|
||||
" if( nodeID < numNodes && (bodies[nodeID].m_invMass != 0.f))\n"
|
||||
" {\n"
|
||||
" if (1)\n"
|
||||
" //angular velocity\n"
|
||||
" {\n"
|
||||
" float4 axis;\n"
|
||||
" //add some hardcoded angular damping\n"
|
||||
@@ -84,9 +84,13 @@ static const char* integrateKernelCL= \
|
||||
" bodies[nodeID].m_quat=predictedOrn;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" //linear velocity \n"
|
||||
" //linear velocity \n"
|
||||
" bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;\n"
|
||||
" \n"
|
||||
" //apply gravity\n"
|
||||
" float4 gravityAcceleration = (float4)(0.f,-9.8f,0.f,0.f);\n"
|
||||
" bodies[nodeID].m_linVel += gravityAcceleration * timeStep;\n"
|
||||
" \n"
|
||||
" }\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
|
||||
195
opencl/gpu_rigidbody/kernels/updateAabbsKernel.cl
Normal file
195
opencl/gpu_rigidbody/kernels/updateAabbsKernel.cl
Normal file
@@ -0,0 +1,195 @@
|
||||
|
||||
#define SHAPE_CONVEX_HULL 3
|
||||
|
||||
typedef float4 Quaternion;
|
||||
|
||||
__inline
|
||||
float4 cross3(float4 a, float4 b)
|
||||
{
|
||||
return cross(a,b);
|
||||
}
|
||||
|
||||
__inline
|
||||
float dot3F4(float4 a, float4 b)
|
||||
{
|
||||
float4 a1 = (float4)(a.xyz,0.f);
|
||||
float4 b1 = (float4)(b.xyz,0.f);
|
||||
return dot(a1, b1);
|
||||
}
|
||||
|
||||
|
||||
__inline
|
||||
Quaternion qtMul(Quaternion a, Quaternion b)
|
||||
{
|
||||
Quaternion ans;
|
||||
ans = cross3( a, b );
|
||||
ans += a.w*b+b.w*a;
|
||||
ans.w = a.w*b.w - dot3F4(a, b);
|
||||
return ans;
|
||||
}
|
||||
|
||||
__inline
|
||||
Quaternion qtInvert(Quaternion q)
|
||||
{
|
||||
return (Quaternion)(-q.xyz, q.w);
|
||||
}
|
||||
|
||||
__inline
|
||||
float4 qtRotate(Quaternion q, float4 vec)
|
||||
{
|
||||
Quaternion qInv = qtInvert( q );
|
||||
float4 vcpy = vec;
|
||||
vcpy.w = 0.f;
|
||||
float4 out = qtMul(qtMul(q,vcpy),qInv);
|
||||
return out;
|
||||
}
|
||||
|
||||
__inline
|
||||
float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
|
||||
{
|
||||
return qtRotate( *orientation, *p ) + (*translation);
|
||||
}
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float4 m_row[3];
|
||||
} Matrix3x3;
|
||||
|
||||
typedef unsigned int u32;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float4 m_pos;
|
||||
float4 m_quat;
|
||||
float4 m_linVel;
|
||||
float4 m_angVel;
|
||||
|
||||
u32 m_collidableIdx;
|
||||
float m_invMass;
|
||||
float m_restituitionCoeff;
|
||||
float m_frictionCoeff;
|
||||
} Body;
|
||||
|
||||
typedef struct Collidable
|
||||
{
|
||||
int m_unused1;
|
||||
int m_unused2;
|
||||
int m_shapeType;
|
||||
int m_shapeIndex;
|
||||
} Collidable;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Matrix3x3 m_invInertia;
|
||||
Matrix3x3 m_initInvInertia;
|
||||
} Shape;
|
||||
|
||||
|
||||
__inline
|
||||
Matrix3x3 qtGetRotationMatrix(float4 quat)
|
||||
{
|
||||
float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);
|
||||
Matrix3x3 out;
|
||||
|
||||
out.m_row[0].x=fabs(1-2*quat2.y-2*quat2.z);
|
||||
out.m_row[0].y=fabs(2*quat.x*quat.y-2*quat.w*quat.z);
|
||||
out.m_row[0].z=fabs(2*quat.x*quat.z+2*quat.w*quat.y);
|
||||
out.m_row[0].w = 0.f;
|
||||
|
||||
out.m_row[1].x=fabs(2*quat.x*quat.y+2*quat.w*quat.z);
|
||||
out.m_row[1].y=fabs(1-2*quat2.x-2*quat2.z);
|
||||
out.m_row[1].z=fabs(2*quat.y*quat.z-2*quat.w*quat.x);
|
||||
out.m_row[1].w = 0.f;
|
||||
|
||||
out.m_row[2].x=fabs(2*quat.x*quat.z-2*quat.w*quat.y);
|
||||
out.m_row[2].y=fabs(2*quat.y*quat.z+2*quat.w*quat.x);
|
||||
out.m_row[2].z=fabs(1-2*quat2.x-2*quat2.y);
|
||||
out.m_row[2].w = 0.f;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float fx;
|
||||
float fy;
|
||||
float fz;
|
||||
int uw;
|
||||
} btAABBCL;
|
||||
|
||||
__inline
|
||||
Matrix3x3 mtTranspose(Matrix3x3 m)
|
||||
{
|
||||
Matrix3x3 out;
|
||||
out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
|
||||
out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
|
||||
out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
|
||||
__inline
|
||||
Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)
|
||||
{
|
||||
Matrix3x3 transB;
|
||||
transB = mtTranspose( b );
|
||||
Matrix3x3 ans;
|
||||
// why this doesn't run when 0ing in the for{}
|
||||
a.m_row[0].w = 0.f;
|
||||
a.m_row[1].w = 0.f;
|
||||
a.m_row[2].w = 0.f;
|
||||
for(int i=0; i<3; i++)
|
||||
{
|
||||
// a.m_row[i].w = 0.f;
|
||||
ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);
|
||||
ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);
|
||||
ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);
|
||||
ans.m_row[i].w = 0.f;
|
||||
}
|
||||
return ans;
|
||||
}
|
||||
|
||||
|
||||
__kernel void initializeGpuAabbsFull( const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global btAABBCL* plocalShapeAABB, __global btAABBCL* pAABB)
|
||||
{
|
||||
int nodeID = get_global_id(0);
|
||||
|
||||
if( nodeID < numNodes )
|
||||
{
|
||||
float4 position = gBodies[nodeID].m_pos;
|
||||
float4 orientation = gBodies[nodeID].m_quat;
|
||||
|
||||
|
||||
int collidableIndex = gBodies[nodeID].m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
|
||||
if (shapeIndex>=0)
|
||||
{
|
||||
btAABBCL minAabb = plocalShapeAABB[collidableIndex*2];
|
||||
btAABBCL maxAabb = plocalShapeAABB[collidableIndex*2+1];
|
||||
|
||||
float4 halfExtents = ((float4)(maxAabb.fx - minAabb.fx,maxAabb.fy - minAabb.fy,maxAabb.fz - minAabb.fz,0.f))*0.5f;
|
||||
float4 localCenter = ((float4)(maxAabb.fx + minAabb.fx,maxAabb.fy + minAabb.fy,maxAabb.fz + minAabb.fz,0.f))*0.5f;
|
||||
|
||||
float4 worldCenter = transform(&localCenter,&position,&orientation);
|
||||
|
||||
Matrix3x3 abs_b = qtGetRotationMatrix(orientation);
|
||||
float4 extent = (float4) ( dot(abs_b.m_row[0],halfExtents),dot(abs_b.m_row[1],halfExtents),dot(abs_b.m_row[2],halfExtents),0.f);
|
||||
|
||||
|
||||
pAABB[nodeID*2].fx = worldCenter.x-extent.x;
|
||||
pAABB[nodeID*2].fy = worldCenter.y-extent.y;
|
||||
pAABB[nodeID*2].fz = worldCenter.z-extent.z;
|
||||
pAABB[nodeID*2].uw = nodeID;
|
||||
|
||||
pAABB[nodeID*2+1].fx = worldCenter.x+extent.x;
|
||||
pAABB[nodeID*2+1].fy = worldCenter.y+extent.y;
|
||||
pAABB[nodeID*2+1].fz = worldCenter.z+extent.z;
|
||||
pAABB[nodeID*2+1].uw = gBodies[nodeID].m_invMass==0.f? 0 : 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
199
opencl/gpu_rigidbody/kernels/updateAabbsKernel.h
Normal file
199
opencl/gpu_rigidbody/kernels/updateAabbsKernel.h
Normal file
@@ -0,0 +1,199 @@
|
||||
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
|
||||
static const char* updateAabbsKernelCL= \
|
||||
"\n"
|
||||
"#define SHAPE_CONVEX_HULL 3\n"
|
||||
"\n"
|
||||
"typedef float4 Quaternion;\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 cross3(float4 a, float4 b)\n"
|
||||
"{\n"
|
||||
" return cross(a,b);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float dot3F4(float4 a, float4 b)\n"
|
||||
"{\n"
|
||||
" float4 a1 = (float4)(a.xyz,0.f);\n"
|
||||
" float4 b1 = (float4)(b.xyz,0.f);\n"
|
||||
" return dot(a1, b1);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtMul(Quaternion a, Quaternion b)\n"
|
||||
"{\n"
|
||||
" Quaternion ans;\n"
|
||||
" ans = cross3( a, b );\n"
|
||||
" ans += a.w*b+b.w*a;\n"
|
||||
" ans.w = a.w*b.w - dot3F4(a, b);\n"
|
||||
" return ans;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Quaternion qtInvert(Quaternion q)\n"
|
||||
"{\n"
|
||||
" return (Quaternion)(-q.xyz, q.w);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 qtRotate(Quaternion q, float4 vec)\n"
|
||||
"{\n"
|
||||
" Quaternion qInv = qtInvert( q );\n"
|
||||
" float4 vcpy = vec;\n"
|
||||
" vcpy.w = 0.f;\n"
|
||||
" float4 out = qtMul(qtMul(q,vcpy),qInv);\n"
|
||||
" return out;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n"
|
||||
"{\n"
|
||||
" return qtRotate( *orientation, *p ) + (*translation);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" float4 m_row[3];\n"
|
||||
"} Matrix3x3;\n"
|
||||
"\n"
|
||||
"typedef unsigned int u32;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" float4 m_pos;\n"
|
||||
" float4 m_quat;\n"
|
||||
" float4 m_linVel;\n"
|
||||
" float4 m_angVel;\n"
|
||||
"\n"
|
||||
" u32 m_collidableIdx;\n"
|
||||
" float m_invMass;\n"
|
||||
" float m_restituitionCoeff;\n"
|
||||
" float m_frictionCoeff;\n"
|
||||
"} Body;\n"
|
||||
"\n"
|
||||
"typedef struct Collidable\n"
|
||||
"{\n"
|
||||
" int m_unused1;\n"
|
||||
" int m_unused2;\n"
|
||||
" int m_shapeType;\n"
|
||||
" int m_shapeIndex;\n"
|
||||
"} Collidable;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" Matrix3x3 m_invInertia;\n"
|
||||
" Matrix3x3 m_initInvInertia;\n"
|
||||
"} Shape;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Matrix3x3 qtGetRotationMatrix(float4 quat)\n"
|
||||
"{\n"
|
||||
" float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n"
|
||||
" Matrix3x3 out;\n"
|
||||
"\n"
|
||||
" out.m_row[0].x=fabs(1-2*quat2.y-2*quat2.z);\n"
|
||||
" out.m_row[0].y=fabs(2*quat.x*quat.y-2*quat.w*quat.z);\n"
|
||||
" out.m_row[0].z=fabs(2*quat.x*quat.z+2*quat.w*quat.y);\n"
|
||||
" out.m_row[0].w = 0.f;\n"
|
||||
"\n"
|
||||
" out.m_row[1].x=fabs(2*quat.x*quat.y+2*quat.w*quat.z);\n"
|
||||
" out.m_row[1].y=fabs(1-2*quat2.x-2*quat2.z);\n"
|
||||
" out.m_row[1].z=fabs(2*quat.y*quat.z-2*quat.w*quat.x);\n"
|
||||
" out.m_row[1].w = 0.f;\n"
|
||||
"\n"
|
||||
" out.m_row[2].x=fabs(2*quat.x*quat.z-2*quat.w*quat.y);\n"
|
||||
" out.m_row[2].y=fabs(2*quat.y*quat.z+2*quat.w*quat.x);\n"
|
||||
" out.m_row[2].z=fabs(1-2*quat2.x-2*quat2.y);\n"
|
||||
" out.m_row[2].w = 0.f;\n"
|
||||
"\n"
|
||||
" return out;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct \n"
|
||||
"{\n"
|
||||
" float fx;\n"
|
||||
" float fy;\n"
|
||||
" float fz;\n"
|
||||
" int uw;\n"
|
||||
"} btAABBCL;\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Matrix3x3 mtTranspose(Matrix3x3 m)\n"
|
||||
"{\n"
|
||||
" Matrix3x3 out;\n"
|
||||
" out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n"
|
||||
" out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n"
|
||||
" out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n"
|
||||
" return out;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline\n"
|
||||
"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n"
|
||||
"{\n"
|
||||
" Matrix3x3 transB;\n"
|
||||
" transB = mtTranspose( b );\n"
|
||||
" Matrix3x3 ans;\n"
|
||||
" // why this doesn't run when 0ing in the for{}\n"
|
||||
" a.m_row[0].w = 0.f;\n"
|
||||
" a.m_row[1].w = 0.f;\n"
|
||||
" a.m_row[2].w = 0.f;\n"
|
||||
" for(int i=0; i<3; i++)\n"
|
||||
" {\n"
|
||||
"// a.m_row[i].w = 0.f;\n"
|
||||
" ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n"
|
||||
" ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n"
|
||||
" ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n"
|
||||
" ans.m_row[i].w = 0.f;\n"
|
||||
" }\n"
|
||||
" return ans;\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__kernel void initializeGpuAabbsFull( const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global btAABBCL* plocalShapeAABB, __global btAABBCL* pAABB)\n"
|
||||
"{\n"
|
||||
" int nodeID = get_global_id(0);\n"
|
||||
" \n"
|
||||
" if( nodeID < numNodes )\n"
|
||||
" {\n"
|
||||
" float4 position = gBodies[nodeID].m_pos;\n"
|
||||
" float4 orientation = gBodies[nodeID].m_quat;\n"
|
||||
" \n"
|
||||
" \n"
|
||||
" int collidableIndex = gBodies[nodeID].m_collidableIdx;\n"
|
||||
" int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n"
|
||||
" \n"
|
||||
" if (shapeIndex>=0)\n"
|
||||
" {\n"
|
||||
" btAABBCL minAabb = plocalShapeAABB[collidableIndex*2];\n"
|
||||
" btAABBCL maxAabb = plocalShapeAABB[collidableIndex*2+1];\n"
|
||||
" \n"
|
||||
" float4 halfExtents = ((float4)(maxAabb.fx - minAabb.fx,maxAabb.fy - minAabb.fy,maxAabb.fz - minAabb.fz,0.f))*0.5f;\n"
|
||||
" float4 localCenter = ((float4)(maxAabb.fx + minAabb.fx,maxAabb.fy + minAabb.fy,maxAabb.fz + minAabb.fz,0.f))*0.5f;\n"
|
||||
" \n"
|
||||
" float4 worldCenter = transform(&localCenter,&position,&orientation);\n"
|
||||
" \n"
|
||||
" Matrix3x3 abs_b = qtGetRotationMatrix(orientation);\n"
|
||||
" float4 extent = (float4) ( dot(abs_b.m_row[0],halfExtents),dot(abs_b.m_row[1],halfExtents),dot(abs_b.m_row[2],halfExtents),0.f);\n"
|
||||
" \n"
|
||||
" \n"
|
||||
" pAABB[nodeID*2].fx = worldCenter.x-extent.x;\n"
|
||||
" pAABB[nodeID*2].fy = worldCenter.y-extent.y;\n"
|
||||
" pAABB[nodeID*2].fz = worldCenter.z-extent.z;\n"
|
||||
" pAABB[nodeID*2].uw = nodeID;\n"
|
||||
" \n"
|
||||
" pAABB[nodeID*2+1].fx = worldCenter.x+extent.x;\n"
|
||||
" pAABB[nodeID*2+1].fy = worldCenter.y+extent.y;\n"
|
||||
" pAABB[nodeID*2+1].fz = worldCenter.z+extent.z;\n"
|
||||
" pAABB[nodeID*2+1].uw = gBodies[nodeID].m_invMass==0.f? 0 : 1;\n"
|
||||
" }\n"
|
||||
" } \n"
|
||||
"}\n"
|
||||
"\n"
|
||||
;
|
||||
Reference in New Issue
Block a user