4409 lines
149 KiB
C++
4409 lines
149 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
bool findSeparatingAxisOnGpu = true;
|
|
bool splitSearchSepAxisConcave = false;
|
|
bool splitSearchSepAxisConvex = true;
|
|
bool useMprGpu = true; //use mpr for edge-edge (+contact point) or sat. Needs testing on main OpenCL platforms, before enabling...
|
|
bool bvhTraversalKernelGPU = true;
|
|
bool findConcaveSeparatingAxisKernelGPU = true;
|
|
bool clipConcaveFacesAndFindContactsCPU = false; //false;//true;
|
|
bool clipConvexFacesAndFindContactsCPU = false; //false;//true;
|
|
bool reduceConcaveContactsOnGPU = true; //false;
|
|
bool reduceConvexContactsOnGPU = true; //false;
|
|
bool findConvexClippingFacesGPU = true;
|
|
bool useGjk = false; ///option for CPU/host testing, when findSeparatingAxisOnGpu = false
|
|
bool useGjkContacts = false; //////option for CPU/host testing when findSeparatingAxisOnGpu = false
|
|
|
|
static int myframecount = 0; ///for testing
|
|
|
|
///This file was written by Erwin Coumans
|
|
///Separating axis rest based on work from Pierre Terdiman, see
|
|
///And contact clipping based on work from Simon Hobbs
|
|
|
|
//#define B3_DEBUG_SAT_FACE
|
|
|
|
//#define CHECK_ON_HOST
|
|
|
|
#ifdef CHECK_ON_HOST
|
|
//#define PERSISTENT_CONTACTS_HOST
|
|
#endif
|
|
|
|
int b3g_actualSATPairTests = 0;
|
|
|
|
#include "b3ConvexHullContact.h"
|
|
#include <string.h> //memcpy
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"
|
|
|
|
#include "Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h"
|
|
#include "Bullet3Geometry/b3AabbUtil.h"
|
|
|
|
typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;
|
|
|
|
#include <float.h> //for FLT_MAX
|
|
#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
|
|
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
|
|
//#include "AdlQuaternion.h"
|
|
|
|
#include "kernels/satKernels.h"
|
|
#include "kernels/mprKernels.h"
|
|
|
|
#include "kernels/satConcaveKernels.h"
|
|
|
|
#include "kernels/satClipHullContacts.h"
|
|
#include "kernels/bvhTraversal.h"
|
|
#include "kernels/primitiveContacts.h"
|
|
|
|
#include "Bullet3Geometry/b3AabbUtil.h"
|
|
|
|
#define BT_NARROWPHASE_SAT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl"
|
|
#define BT_NARROWPHASE_SAT_CONCAVE_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl"
|
|
|
|
#define BT_NARROWPHASE_MPR_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl"
|
|
|
|
#define BT_NARROWPHASE_CLIPHULL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl"
|
|
#define BT_NARROWPHASE_BVH_TRAVERSAL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl"
|
|
#define BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl"
|
|
|
|
#ifndef __global
|
|
#define __global
|
|
#endif
|
|
|
|
#ifndef __kernel
|
|
#define __kernel
|
|
#endif
|
|
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h"
|
|
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h"
|
|
|
|
#define dot3F4 b3Dot
|
|
|
|
GpuSatCollision::GpuSatCollision(cl_context ctx, cl_device_id device, cl_command_queue q)
|
|
: m_context(ctx),
|
|
m_device(device),
|
|
m_queue(q),
|
|
|
|
m_findSeparatingAxisKernel(0),
|
|
m_findSeparatingAxisVertexFaceKernel(0),
|
|
m_findSeparatingAxisEdgeEdgeKernel(0),
|
|
m_unitSphereDirections(m_context, m_queue),
|
|
|
|
m_totalContactsOut(m_context, m_queue),
|
|
m_sepNormals(m_context, m_queue),
|
|
m_dmins(m_context, m_queue),
|
|
|
|
m_hasSeparatingNormals(m_context, m_queue),
|
|
m_concaveSepNormals(m_context, m_queue),
|
|
m_concaveHasSeparatingNormals(m_context, m_queue),
|
|
m_numConcavePairsOut(m_context, m_queue),
|
|
|
|
m_gpuCompoundPairs(m_context, m_queue),
|
|
|
|
m_gpuCompoundSepNormals(m_context, m_queue),
|
|
m_gpuHasCompoundSepNormals(m_context, m_queue),
|
|
|
|
m_numCompoundPairsOut(m_context, m_queue)
|
|
{
|
|
m_totalContactsOut.push_back(0);
|
|
|
|
cl_int errNum = 0;
|
|
|
|
if (1)
|
|
{
|
|
const char* mprSrc = mprKernelsCL;
|
|
|
|
const char* srcConcave = satConcaveKernelsCL;
|
|
char flags[1024] = {0};
|
|
//#ifdef CL_PLATFORM_INTEL
|
|
// sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/sat.cl");
|
|
//#endif
|
|
m_mprPenetrationKernel = 0;
|
|
m_findSeparatingAxisUnitSphereKernel = 0;
|
|
|
|
if (useMprGpu)
|
|
{
|
|
cl_program mprProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, mprSrc, &errNum, flags, BT_NARROWPHASE_MPR_PATH);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_mprPenetrationKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, mprSrc, "mprPenetrationKernel", &errNum, mprProg);
|
|
b3Assert(m_mprPenetrationKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findSeparatingAxisUnitSphereKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, mprSrc, "findSeparatingAxisUnitSphereKernel", &errNum, mprProg);
|
|
b3Assert(m_findSeparatingAxisUnitSphereKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
|
|
m_unitSphereDirections.resize(numDirections);
|
|
m_unitSphereDirections.copyFromHostPointer(unitSphere162, numDirections, 0, true);
|
|
}
|
|
|
|
cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, satKernelsCL, &errNum, flags, BT_NARROWPHASE_SAT_PATH);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
cl_program satConcaveProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcConcave, &errNum, flags, BT_NARROWPHASE_SAT_CONCAVE_PATH);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisKernel", &errNum, satProg);
|
|
b3Assert(m_findSeparatingAxisKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisVertexFaceKernel", &errNum, satProg);
|
|
b3Assert(m_findSeparatingAxisVertexFaceKernel);
|
|
|
|
m_findSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findSeparatingAxisEdgeEdgeKernel", &errNum, satProg);
|
|
b3Assert(m_findSeparatingAxisVertexFaceKernel);
|
|
|
|
m_findConcaveSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findConcaveSeparatingAxisKernel", &errNum, satProg);
|
|
b3Assert(m_findConcaveSeparatingAxisKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findConcaveSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcConcave, "findConcaveSeparatingAxisVertexFaceKernel", &errNum, satConcaveProg);
|
|
b3Assert(m_findConcaveSeparatingAxisVertexFaceKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findConcaveSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcConcave, "findConcaveSeparatingAxisEdgeEdgeKernel", &errNum, satConcaveProg);
|
|
b3Assert(m_findConcaveSeparatingAxisEdgeEdgeKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "findCompoundPairsKernel", &errNum, satProg);
|
|
b3Assert(m_findCompoundPairsKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
m_processCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, satKernelsCL, "processCompoundPairsKernel", &errNum, satProg);
|
|
b3Assert(m_processCompoundPairsKernel);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
}
|
|
|
|
if (1)
|
|
{
|
|
const char* srcClip = satClipKernelsCL;
|
|
|
|
char flags[1024] = {0};
|
|
//#ifdef CL_PLATFORM_INTEL
|
|
// sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/satClipHullContacts.cl");
|
|
//#endif
|
|
|
|
cl_program satClipContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcClip, &errNum, flags, BT_NARROWPHASE_CLIPHULL_PATH);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_clipHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipHullHullKernel", &errNum, satClipContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_clipCompoundsHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipCompoundsHullHullKernel", &errNum, satClipContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findClippingFacesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "findClippingFacesKernel", &errNum, satClipContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_clipFacesAndFindContacts = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipFacesAndFindContactsKernel", &errNum, satClipContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_clipHullHullConcaveConvexKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip, "clipHullHullConcaveConvexKernel", &errNum, satClipContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
// m_extractManifoldAndAddContactKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "extractManifoldAndAddContactKernel",&errNum,satClipContactsProg);
|
|
// b3Assert(errNum==CL_SUCCESS);
|
|
|
|
m_newContactReductionKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcClip,
|
|
"newContactReductionKernel", &errNum, satClipContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
}
|
|
else
|
|
{
|
|
m_clipHullHullKernel = 0;
|
|
m_clipCompoundsHullHullKernel = 0;
|
|
m_findClippingFacesKernel = 0;
|
|
m_newContactReductionKernel = 0;
|
|
m_clipFacesAndFindContacts = 0;
|
|
m_clipHullHullConcaveConvexKernel = 0;
|
|
// m_extractManifoldAndAddContactKernel = 0;
|
|
}
|
|
|
|
if (1)
|
|
{
|
|
const char* srcBvh = bvhTraversalKernelCL;
|
|
cl_program bvhTraversalProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, srcBvh, &errNum, "", BT_NARROWPHASE_BVH_TRAVERSAL_PATH);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_bvhTraversalKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, srcBvh, "bvhTraversalKernel", &errNum, bvhTraversalProg, "");
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
}
|
|
|
|
{
|
|
const char* primitiveContactsSrc = primitiveContactsKernelsCL;
|
|
cl_program primitiveContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, primitiveContactsSrc, &errNum, "", BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_primitiveContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "primitiveContactsKernel", &errNum, primitiveContactsProg, "");
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
|
|
m_findConcaveSphereContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "findConcaveSphereContactsKernel", &errNum, primitiveContactsProg);
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
b3Assert(m_findConcaveSphereContactsKernel);
|
|
|
|
m_processCompoundPairsPrimitivesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, primitiveContactsSrc, "processCompoundPairsPrimitivesKernel", &errNum, primitiveContactsProg, "");
|
|
b3Assert(errNum == CL_SUCCESS);
|
|
b3Assert(m_processCompoundPairsPrimitivesKernel);
|
|
}
|
|
}
|
|
|
|
GpuSatCollision::~GpuSatCollision()
|
|
{
|
|
if (m_findSeparatingAxisVertexFaceKernel)
|
|
clReleaseKernel(m_findSeparatingAxisVertexFaceKernel);
|
|
|
|
if (m_findSeparatingAxisEdgeEdgeKernel)
|
|
clReleaseKernel(m_findSeparatingAxisEdgeEdgeKernel);
|
|
|
|
if (m_findSeparatingAxisUnitSphereKernel)
|
|
clReleaseKernel(m_findSeparatingAxisUnitSphereKernel);
|
|
|
|
if (m_mprPenetrationKernel)
|
|
clReleaseKernel(m_mprPenetrationKernel);
|
|
|
|
if (m_findSeparatingAxisKernel)
|
|
clReleaseKernel(m_findSeparatingAxisKernel);
|
|
|
|
if (m_findConcaveSeparatingAxisVertexFaceKernel)
|
|
clReleaseKernel(m_findConcaveSeparatingAxisVertexFaceKernel);
|
|
|
|
if (m_findConcaveSeparatingAxisEdgeEdgeKernel)
|
|
clReleaseKernel(m_findConcaveSeparatingAxisEdgeEdgeKernel);
|
|
|
|
if (m_findConcaveSeparatingAxisKernel)
|
|
clReleaseKernel(m_findConcaveSeparatingAxisKernel);
|
|
|
|
if (m_findCompoundPairsKernel)
|
|
clReleaseKernel(m_findCompoundPairsKernel);
|
|
|
|
if (m_processCompoundPairsKernel)
|
|
clReleaseKernel(m_processCompoundPairsKernel);
|
|
|
|
if (m_findClippingFacesKernel)
|
|
clReleaseKernel(m_findClippingFacesKernel);
|
|
|
|
if (m_clipFacesAndFindContacts)
|
|
clReleaseKernel(m_clipFacesAndFindContacts);
|
|
if (m_newContactReductionKernel)
|
|
clReleaseKernel(m_newContactReductionKernel);
|
|
if (m_primitiveContactsKernel)
|
|
clReleaseKernel(m_primitiveContactsKernel);
|
|
|
|
if (m_findConcaveSphereContactsKernel)
|
|
clReleaseKernel(m_findConcaveSphereContactsKernel);
|
|
|
|
if (m_processCompoundPairsPrimitivesKernel)
|
|
clReleaseKernel(m_processCompoundPairsPrimitivesKernel);
|
|
|
|
if (m_clipHullHullKernel)
|
|
clReleaseKernel(m_clipHullHullKernel);
|
|
if (m_clipCompoundsHullHullKernel)
|
|
clReleaseKernel(m_clipCompoundsHullHullKernel);
|
|
|
|
if (m_clipHullHullConcaveConvexKernel)
|
|
clReleaseKernel(m_clipHullHullConcaveConvexKernel);
|
|
// if (m_extractManifoldAndAddContactKernel)
|
|
// clReleaseKernel(m_extractManifoldAndAddContactKernel);
|
|
|
|
if (m_bvhTraversalKernel)
|
|
clReleaseKernel(m_bvhTraversalKernel);
|
|
}
|
|
|
|
struct MyTriangleCallback : public b3NodeOverlapCallback
|
|
{
|
|
int m_bodyIndexA;
|
|
int m_bodyIndexB;
|
|
|
|
virtual void processNode(int subPart, int triangleIndex)
|
|
{
|
|
printf("bodyIndexA %d, bodyIndexB %d\n", m_bodyIndexA, m_bodyIndexB);
|
|
printf("triangleIndex %d\n", triangleIndex);
|
|
}
|
|
};
|
|
|
|
#define float4 b3Vector3
|
|
#define make_float4(x, y, z, w) b3MakeVector3(x, y, z, w)
|
|
|
|
float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn, float4* closestPointOnFace)
|
|
{
|
|
float4 n = planeEqn;
|
|
n[3] = 0.f;
|
|
float dist = dot3F4(n, point) + planeEqn[3];
|
|
*closestPointOnFace = point - dist * n;
|
|
return dist;
|
|
}
|
|
|
|
#define cross3(a, b) (a.cross(b))
|
|
b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn)
|
|
{
|
|
b3Transform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(*pos);
|
|
tr.setRotation(*orn);
|
|
b3Vector3 res = tr(*v);
|
|
return res;
|
|
}
|
|
|
|
inline bool IsPointInPolygon(const float4& p,
|
|
const b3GpuFace* face,
|
|
const float4* baseVertex,
|
|
const int* convexIndices,
|
|
float4* out)
|
|
{
|
|
float4 a;
|
|
float4 b;
|
|
float4 ab;
|
|
float4 ap;
|
|
float4 v;
|
|
|
|
float4 plane = b3MakeVector3(face->m_plane.x, face->m_plane.y, face->m_plane.z, 0.f);
|
|
|
|
if (face->m_numIndices < 2)
|
|
return false;
|
|
|
|
float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices - 1]];
|
|
b = v0;
|
|
|
|
for (unsigned i = 0; i != face->m_numIndices; ++i)
|
|
{
|
|
a = b;
|
|
float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
|
|
b = vi;
|
|
ab = b - a;
|
|
ap = p - a;
|
|
v = cross3(ab, plane);
|
|
|
|
if (b3Dot(ap, v) > 0.f)
|
|
{
|
|
float ab_m2 = b3Dot(ab, ab);
|
|
float rt = ab_m2 != 0.f ? b3Dot(ab, ap) / ab_m2 : 0.f;
|
|
if (rt <= 0.f)
|
|
{
|
|
*out = a;
|
|
}
|
|
else if (rt >= 1.f)
|
|
{
|
|
*out = b;
|
|
}
|
|
else
|
|
{
|
|
float s = 1.f - rt;
|
|
out[0].x = s * a.x + rt * b.x;
|
|
out[0].y = s * a.y + rt * b.y;
|
|
out[0].z = s * a.z + rt * b.z;
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
#define normalize3(a) (a.normalize())
|
|
|
|
int extractManifoldSequentialGlobal(const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx)
|
|
{
|
|
if (nPoints == 0)
|
|
return 0;
|
|
|
|
if (nPoints <= 4)
|
|
return nPoints;
|
|
|
|
if (nPoints > 64)
|
|
nPoints = 64;
|
|
|
|
float4 center = b3MakeVector3(0, 0, 0, 0);
|
|
{
|
|
for (int i = 0; i < nPoints; i++)
|
|
center += p[i];
|
|
center /= (float)nPoints;
|
|
}
|
|
|
|
// sample 4 directions
|
|
|
|
float4 aVector = p[0] - center;
|
|
float4 u = cross3(nearNormal, aVector);
|
|
float4 v = cross3(nearNormal, u);
|
|
u = normalize3(u);
|
|
v = normalize3(v);
|
|
|
|
//keep point with deepest penetration
|
|
float minW = FLT_MAX;
|
|
|
|
int minIndex = -1;
|
|
|
|
float4 maxDots;
|
|
maxDots.x = FLT_MIN;
|
|
maxDots.y = FLT_MIN;
|
|
maxDots.z = FLT_MIN;
|
|
maxDots.w = FLT_MIN;
|
|
|
|
// idx, distance
|
|
for (int ie = 0; ie < nPoints; ie++)
|
|
{
|
|
if (p[ie].w < minW)
|
|
{
|
|
minW = p[ie].w;
|
|
minIndex = ie;
|
|
}
|
|
float f;
|
|
float4 r = p[ie] - center;
|
|
f = dot3F4(u, r);
|
|
if (f < maxDots.x)
|
|
{
|
|
maxDots.x = f;
|
|
contactIdx[0].x = ie;
|
|
}
|
|
|
|
f = dot3F4(-u, r);
|
|
if (f < maxDots.y)
|
|
{
|
|
maxDots.y = f;
|
|
contactIdx[0].y = ie;
|
|
}
|
|
|
|
f = dot3F4(v, r);
|
|
if (f < maxDots.z)
|
|
{
|
|
maxDots.z = f;
|
|
contactIdx[0].z = ie;
|
|
}
|
|
|
|
f = dot3F4(-v, r);
|
|
if (f < maxDots.w)
|
|
{
|
|
maxDots.w = f;
|
|
contactIdx[0].w = ie;
|
|
}
|
|
}
|
|
|
|
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
|
{
|
|
//replace the first contact with minimum (todo: replace contact with least penetration)
|
|
contactIdx[0].x = minIndex;
|
|
}
|
|
|
|
return 4;
|
|
}
|
|
|
|
#define MAX_VERTS 1024
|
|
|
|
inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
|
{
|
|
min = FLT_MAX;
|
|
max = -FLT_MAX;
|
|
int numVerts = hull.m_numVertices;
|
|
|
|
const float4 localDir = b3QuatRotate(orn.inverse(), dir);
|
|
|
|
b3Scalar offset = dot3F4(pos, dir);
|
|
|
|
for (int i = 0; i < numVerts; i++)
|
|
{
|
|
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
|
|
//b3Scalar dp = pt.dot(dir);
|
|
//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
|
|
b3Scalar dp = dot3F4((float4&)vertices[hull.m_vertexOffset + i], localDir);
|
|
//b3Assert(dp==dpL);
|
|
if (dp < min) min = dp;
|
|
if (dp > max) max = dp;
|
|
}
|
|
if (min > max)
|
|
{
|
|
b3Scalar tmp = min;
|
|
min = max;
|
|
max = tmp;
|
|
}
|
|
min += offset;
|
|
max += offset;
|
|
}
|
|
|
|
static bool TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
|
const float4& posA, const b3Quaternion& ornA,
|
|
const float4& posB, const b3Quaternion& ornB,
|
|
const float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
|
|
{
|
|
b3Scalar Min0, Max0;
|
|
b3Scalar Min1, Max1;
|
|
project(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
|
|
project(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
|
|
|
|
if (Max0 < Min1 || Max1 < Min0)
|
|
return false;
|
|
|
|
b3Scalar d0 = Max0 - Min1;
|
|
assert(d0 >= 0.0f);
|
|
b3Scalar d1 = Max1 - Min0;
|
|
assert(d1 >= 0.0f);
|
|
depth = d0 < d1 ? d0 : d1;
|
|
return true;
|
|
}
|
|
|
|
inline bool IsAlmostZero(const b3Vector3& v)
|
|
{
|
|
if (fabsf(v.x) > 1e-6 || fabsf(v.y) > 1e-6 || fabsf(v.z) > 1e-6) return false;
|
|
return true;
|
|
}
|
|
|
|
static bool findSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
|
const float4& posA1,
|
|
const b3Quaternion& ornA,
|
|
const float4& posB1,
|
|
const b3Quaternion& ornB,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
|
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
|
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
|
const b3AlignedObjectArray<int>& indicesA,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
|
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
|
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
|
const b3AlignedObjectArray<int>& indicesB,
|
|
|
|
b3Vector3& sep)
|
|
{
|
|
B3_PROFILE("findSeparatingAxis");
|
|
|
|
b3g_actualSATPairTests++;
|
|
float4 posA = posA1;
|
|
posA.w = 0.f;
|
|
float4 posB = posB1;
|
|
posB.w = 0.f;
|
|
//#ifdef TEST_INTERNAL_OBJECTS
|
|
float4 c0local = (float4&)hullA.m_localCenter;
|
|
float4 c0 = transform(&c0local, &posA, &ornA);
|
|
float4 c1local = (float4&)hullB.m_localCenter;
|
|
float4 c1 = transform(&c1local, &posB, &ornB);
|
|
const float4 deltaC2 = c0 - c1;
|
|
//#endif
|
|
|
|
b3Scalar dmin = FLT_MAX;
|
|
int curPlaneTests = 0;
|
|
|
|
int numFacesA = hullA.m_numFaces;
|
|
// Test normals from hullA
|
|
for (int i = 0; i < numFacesA; i++)
|
|
{
|
|
const float4& normal = (float4&)facesA[hullA.m_faceOffset + i].m_plane;
|
|
float4 faceANormalWS = b3QuatRotate(ornA, normal);
|
|
|
|
if (dot3F4(deltaC2, faceANormalWS) < 0)
|
|
faceANormalWS *= -1.f;
|
|
|
|
curPlaneTests++;
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
gExpectedNbTests++;
|
|
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
|
|
continue;
|
|
gActualNbTests++;
|
|
#endif
|
|
|
|
b3Scalar d;
|
|
if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
|
|
return false;
|
|
|
|
if (d < dmin)
|
|
{
|
|
dmin = d;
|
|
sep = (b3Vector3&)faceANormalWS;
|
|
}
|
|
}
|
|
|
|
int numFacesB = hullB.m_numFaces;
|
|
// Test normals from hullB
|
|
for (int i = 0; i < numFacesB; i++)
|
|
{
|
|
float4 normal = (float4&)facesB[hullB.m_faceOffset + i].m_plane;
|
|
float4 WorldNormal = b3QuatRotate(ornB, normal);
|
|
|
|
if (dot3F4(deltaC2, WorldNormal) < 0)
|
|
{
|
|
WorldNormal *= -1.f;
|
|
}
|
|
curPlaneTests++;
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
gExpectedNbTests++;
|
|
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
|
|
continue;
|
|
gActualNbTests++;
|
|
#endif
|
|
|
|
b3Scalar d;
|
|
if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
|
|
return false;
|
|
|
|
if (d < dmin)
|
|
{
|
|
dmin = d;
|
|
sep = (b3Vector3&)WorldNormal;
|
|
}
|
|
}
|
|
|
|
int curEdgeEdge = 0;
|
|
// Test edges
|
|
for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
|
|
{
|
|
const float4& edge0 = (float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
|
|
float4 edge0World = b3QuatRotate(ornA, (float4&)edge0);
|
|
|
|
for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
|
|
{
|
|
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
|
|
float4 edge1World = b3QuatRotate(ornB, (float4&)edge1);
|
|
|
|
float4 crossje = cross3(edge0World, edge1World);
|
|
|
|
curEdgeEdge++;
|
|
if (!IsAlmostZero((b3Vector3&)crossje))
|
|
{
|
|
crossje = normalize3(crossje);
|
|
if (dot3F4(deltaC2, crossje) < 0)
|
|
crossje *= -1.f;
|
|
|
|
#ifdef TEST_INTERNAL_OBJECTS
|
|
gExpectedNbTests++;
|
|
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
|
|
continue;
|
|
gActualNbTests++;
|
|
#endif
|
|
|
|
b3Scalar dist;
|
|
if (!TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
|
|
return false;
|
|
|
|
if (dist < dmin)
|
|
{
|
|
dmin = dist;
|
|
sep = (b3Vector3&)crossje;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if ((dot3F4(-deltaC2, (float4&)sep)) > 0.0f)
|
|
sep = -sep;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool findSeparatingAxisEdgeEdge(__global const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
|
const b3Float4& posA1,
|
|
const b3Quat& ornA,
|
|
const b3Float4& posB1,
|
|
const b3Quat& ornB,
|
|
const b3Float4& DeltaC2,
|
|
__global const b3AlignedObjectArray<float4>& vertices,
|
|
__global const b3AlignedObjectArray<float4>& uniqueEdges,
|
|
__global const b3AlignedObjectArray<b3GpuFace>& faces,
|
|
__global const b3AlignedObjectArray<int>& indices,
|
|
float4* sep,
|
|
float* dmin)
|
|
{
|
|
// int i = get_global_id(0);
|
|
|
|
float4 posA = posA1;
|
|
posA.w = 0.f;
|
|
float4 posB = posB1;
|
|
posB.w = 0.f;
|
|
|
|
//int curPlaneTests=0;
|
|
|
|
int curEdgeEdge = 0;
|
|
// Test edges
|
|
for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
|
|
{
|
|
const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset + e0];
|
|
float4 edge0World = b3QuatRotate(ornA, edge0);
|
|
|
|
for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
|
|
{
|
|
const float4 edge1 = uniqueEdges[hullB->m_uniqueEdgesOffset + e1];
|
|
float4 edge1World = b3QuatRotate(ornB, edge1);
|
|
|
|
float4 crossje = cross3(edge0World, edge1World);
|
|
|
|
curEdgeEdge++;
|
|
if (!IsAlmostZero(crossje))
|
|
{
|
|
crossje = normalize3(crossje);
|
|
if (dot3F4(DeltaC2, crossje) < 0)
|
|
crossje *= -1.f;
|
|
|
|
float dist;
|
|
bool result = true;
|
|
{
|
|
float Min0, Max0;
|
|
float Min1, Max1;
|
|
project(*hullA, posA, ornA, crossje, vertices, Min0, Max0);
|
|
project(*hullB, posB, ornB, crossje, vertices, Min1, Max1);
|
|
|
|
if (Max0 < Min1 || Max1 < Min0)
|
|
result = false;
|
|
|
|
float d0 = Max0 - Min1;
|
|
float d1 = Max1 - Min0;
|
|
dist = d0 < d1 ? d0 : d1;
|
|
result = true;
|
|
}
|
|
|
|
if (dist < *dmin)
|
|
{
|
|
*dmin = dist;
|
|
*sep = crossje;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if ((dot3F4(-DeltaC2, *sep)) > 0.0f)
|
|
{
|
|
*sep = -(*sep);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
__inline float4 lerp3(const float4& a, const float4& b, float t)
|
|
{
|
|
return b3MakeVector3(a.x + (b.x - a.x) * t,
|
|
a.y + (b.y - a.y) * t,
|
|
a.z + (b.z - a.z) * t,
|
|
0.f);
|
|
}
|
|
|
|
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
|
int clipFace(const float4* pVtxIn, int numVertsIn, float4& planeNormalWS, float planeEqWS, float4* ppVtxOut)
|
|
{
|
|
int ve;
|
|
float ds, de;
|
|
int numVertsOut = 0;
|
|
if (numVertsIn < 2)
|
|
return 0;
|
|
|
|
float4 firstVertex = pVtxIn[numVertsIn - 1];
|
|
float4 endVertex = pVtxIn[0];
|
|
|
|
ds = dot3F4(planeNormalWS, firstVertex) + planeEqWS;
|
|
|
|
for (ve = 0; ve < numVertsIn; ve++)
|
|
{
|
|
endVertex = pVtxIn[ve];
|
|
|
|
de = dot3F4(planeNormalWS, endVertex) + planeEqWS;
|
|
|
|
if (ds < 0)
|
|
{
|
|
if (de < 0)
|
|
{
|
|
// Start < 0, end < 0, so output endVertex
|
|
ppVtxOut[numVertsOut++] = endVertex;
|
|
}
|
|
else
|
|
{
|
|
// Start < 0, end >= 0, so output intersection
|
|
ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (de < 0)
|
|
{
|
|
// Start >= 0, end < 0 so output intersection and end
|
|
ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
|
ppVtxOut[numVertsOut++] = endVertex;
|
|
}
|
|
}
|
|
firstVertex = endVertex;
|
|
ds = de;
|
|
}
|
|
return numVertsOut;
|
|
}
|
|
|
|
int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
|
|
const float4& posA, const b3Quaternion& ornA, float4* worldVertsB1, int numWorldVertsB1,
|
|
float4* worldVertsB2, int capacityWorldVertsB2,
|
|
const float minDist, float maxDist,
|
|
const b3AlignedObjectArray<float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
|
//const float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
|
|
float4* contactsOut,
|
|
int contactCapacity)
|
|
{
|
|
int numContactsOut = 0;
|
|
|
|
float4* pVtxIn = worldVertsB1;
|
|
float4* pVtxOut = worldVertsB2;
|
|
|
|
int numVertsIn = numWorldVertsB1;
|
|
int numVertsOut = 0;
|
|
|
|
int closestFaceA = -1;
|
|
{
|
|
float dmin = FLT_MAX;
|
|
for (int face = 0; face < hullA->m_numFaces; face++)
|
|
{
|
|
const float4 Normal = b3MakeVector3(
|
|
facesA[hullA->m_faceOffset + face].m_plane.x,
|
|
facesA[hullA->m_faceOffset + face].m_plane.y,
|
|
facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
|
|
const float4 faceANormalWS = b3QuatRotate(ornA, Normal);
|
|
|
|
float d = dot3F4(faceANormalWS, separatingNormal);
|
|
if (d < dmin)
|
|
{
|
|
dmin = d;
|
|
closestFaceA = face;
|
|
}
|
|
}
|
|
}
|
|
if (closestFaceA < 0)
|
|
return numContactsOut;
|
|
|
|
b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
|
|
|
|
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
|
// int numContacts = numWorldVertsB1;
|
|
int numVerticesA = polyA.m_numIndices;
|
|
for (int e0 = 0; e0 < numVerticesA; e0++)
|
|
{
|
|
const float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
|
|
const float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
|
|
const float4 edge0 = a - b;
|
|
const float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
|
|
float4 planeNormalA = make_float4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
|
|
float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
|
|
|
|
float4 planeNormalWS1 = -cross3(WorldEdge0, worldPlaneAnormal1);
|
|
float4 worldA1 = transform(&a, &posA, &ornA);
|
|
float planeEqWS1 = -dot3F4(worldA1, planeNormalWS1);
|
|
|
|
float4 planeNormalWS = planeNormalWS1;
|
|
float planeEqWS = planeEqWS1;
|
|
|
|
//clip face
|
|
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
|
|
numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
|
|
|
|
//btSwap(pVtxIn,pVtxOut);
|
|
float4* tmp = pVtxOut;
|
|
pVtxOut = pVtxIn;
|
|
pVtxIn = tmp;
|
|
numVertsIn = numVertsOut;
|
|
numVertsOut = 0;
|
|
}
|
|
|
|
// only keep points that are behind the witness face
|
|
{
|
|
float4 localPlaneNormal = make_float4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
|
|
float localPlaneEq = polyA.m_plane.w;
|
|
float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
|
|
float planeEqWS = localPlaneEq - dot3F4(planeNormalWS, posA);
|
|
for (int i = 0; i < numVertsIn; i++)
|
|
{
|
|
float depth = dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
|
|
if (depth <= minDist)
|
|
{
|
|
depth = minDist;
|
|
}
|
|
if (numContactsOut < contactCapacity)
|
|
{
|
|
if (depth <= maxDist)
|
|
{
|
|
float4 pointInWorld = pVtxIn[i];
|
|
//resultOut.addContactPoint(separatingNormal,point,depth);
|
|
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
|
|
//printf("depth=%f\n",depth);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
|
|
}
|
|
}
|
|
}
|
|
|
|
return numContactsOut;
|
|
}
|
|
|
|
static int clipHullAgainstHull(const float4& separatingNormal,
|
|
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
|
const float4& posA, const b3Quaternion& ornA, const float4& posB, const b3Quaternion& ornB,
|
|
float4* worldVertsB1, float4* worldVertsB2, int capacityWorldVerts,
|
|
const float minDist, float maxDist,
|
|
const b3AlignedObjectArray<float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
|
const b3AlignedObjectArray<float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
|
|
|
|
float4* contactsOut,
|
|
int contactCapacity)
|
|
{
|
|
int numContactsOut = 0;
|
|
int numWorldVertsB1 = 0;
|
|
|
|
B3_PROFILE("clipHullAgainstHull");
|
|
|
|
// float curMaxDist=maxDist;
|
|
int closestFaceB = -1;
|
|
float dmax = -FLT_MAX;
|
|
|
|
{
|
|
//B3_PROFILE("closestFaceB");
|
|
if (hullB.m_numFaces != 1)
|
|
{
|
|
//printf("wtf\n");
|
|
}
|
|
static bool once = true;
|
|
//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
|
|
|
|
for (int face = 0; face < hullB.m_numFaces; face++)
|
|
{
|
|
#ifdef BT_DEBUG_SAT_FACE
|
|
if (once)
|
|
printf("face %d\n", face);
|
|
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
|
|
if (once)
|
|
{
|
|
for (int i = 0; i < faceB->m_numIndices; i++)
|
|
{
|
|
float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
|
|
printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
|
|
}
|
|
}
|
|
#endif //BT_DEBUG_SAT_FACE \
|
|
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
|
|
{
|
|
const float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
|
|
facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
|
|
const float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
|
#ifdef BT_DEBUG_SAT_FACE
|
|
if (once)
|
|
printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
|
|
#endif
|
|
float d = dot3F4(WorldNormal, separatingNormal);
|
|
if (d > dmax)
|
|
{
|
|
dmax = d;
|
|
closestFaceB = face;
|
|
}
|
|
}
|
|
}
|
|
once = false;
|
|
}
|
|
|
|
b3Assert(closestFaceB >= 0);
|
|
{
|
|
//B3_PROFILE("worldVertsB1");
|
|
const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
|
|
const int numVertices = polyB.m_numIndices;
|
|
for (int e0 = 0; e0 < numVertices; e0++)
|
|
{
|
|
const float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
|
|
worldVertsB1[numWorldVertsB1++] = transform(&b, &posB, &ornB);
|
|
}
|
|
}
|
|
|
|
if (closestFaceB >= 0)
|
|
{
|
|
//B3_PROFILE("clipFaceAgainstHull");
|
|
numContactsOut = clipFaceAgainstHull((float4&)separatingNormal, &hullA,
|
|
posA, ornA,
|
|
worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
|
|
verticesA, facesA, indicesA,
|
|
contactsOut, contactCapacity);
|
|
}
|
|
|
|
return numContactsOut;
|
|
}
|
|
|
|
#define PARALLEL_SUM(v, n) \
|
|
for (int j = 1; j < n; j++) v[0] += v[j];
|
|
#define PARALLEL_DO(execution, n) \
|
|
for (int ie = 0; ie < n; ie++) \
|
|
{ \
|
|
execution; \
|
|
}
|
|
#define REDUCE_MAX(v, n) \
|
|
{ \
|
|
int i = 0; \
|
|
for (int offset = 0; offset < n; offset++) v[i] = (v[i].y > v[i + offset].y) ? v[i] : v[i + offset]; \
|
|
}
|
|
#define REDUCE_MIN(v, n) \
|
|
{ \
|
|
int i = 0; \
|
|
for (int offset = 0; offset < n; offset++) v[i] = (v[i].y < v[i + offset].y) ? v[i] : v[i + offset]; \
|
|
}
|
|
|
|
int extractManifold(const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx)
|
|
{
|
|
if (nPoints == 0)
|
|
return 0;
|
|
|
|
if (nPoints <= 4)
|
|
return nPoints;
|
|
|
|
if (nPoints > 64)
|
|
nPoints = 64;
|
|
|
|
float4 center = make_float4(0, 0, 0, 0);
|
|
{
|
|
for (int i = 0; i < nPoints; i++)
|
|
center += p[i];
|
|
center /= (float)nPoints;
|
|
}
|
|
|
|
// sample 4 directions
|
|
|
|
float4 aVector = p[0] - center;
|
|
float4 u = cross3(nearNormal, aVector);
|
|
float4 v = cross3(nearNormal, u);
|
|
u = normalize3(u);
|
|
v = normalize3(v);
|
|
|
|
//keep point with deepest penetration
|
|
float minW = FLT_MAX;
|
|
|
|
int minIndex = -1;
|
|
|
|
float4 maxDots;
|
|
maxDots.x = FLT_MIN;
|
|
maxDots.y = FLT_MIN;
|
|
maxDots.z = FLT_MIN;
|
|
maxDots.w = FLT_MIN;
|
|
|
|
// idx, distance
|
|
for (int ie = 0; ie < nPoints; ie++)
|
|
{
|
|
if (p[ie].w < minW)
|
|
{
|
|
minW = p[ie].w;
|
|
minIndex = ie;
|
|
}
|
|
float f;
|
|
float4 r = p[ie] - center;
|
|
f = dot3F4(u, r);
|
|
if (f < maxDots.x)
|
|
{
|
|
maxDots.x = f;
|
|
contactIdx[0].x = ie;
|
|
}
|
|
|
|
f = dot3F4(-u, r);
|
|
if (f < maxDots.y)
|
|
{
|
|
maxDots.y = f;
|
|
contactIdx[0].y = ie;
|
|
}
|
|
|
|
f = dot3F4(v, r);
|
|
if (f < maxDots.z)
|
|
{
|
|
maxDots.z = f;
|
|
contactIdx[0].z = ie;
|
|
}
|
|
|
|
f = dot3F4(-v, r);
|
|
if (f < maxDots.w)
|
|
{
|
|
maxDots.w = f;
|
|
contactIdx[0].w = ie;
|
|
}
|
|
}
|
|
|
|
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
|
{
|
|
//replace the first contact with minimum (todo: replace contact with least penetration)
|
|
contactIdx[0].x = minIndex;
|
|
}
|
|
|
|
return 4;
|
|
}
|
|
|
|
int clipHullHullSingle(
|
|
int bodyIndexA, int bodyIndexB,
|
|
const float4& posA,
|
|
const b3Quaternion& ornA,
|
|
const float4& posB,
|
|
const b3Quaternion& ornB,
|
|
|
|
int collidableIndexA, int collidableIndexB,
|
|
|
|
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
|
|
b3AlignedObjectArray<b3Contact4>* globalContactOut,
|
|
int& nContacts,
|
|
|
|
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
|
|
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
|
|
|
|
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
|
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
|
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
|
const b3AlignedObjectArray<int>& indicesA,
|
|
|
|
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
|
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
|
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
|
const b3AlignedObjectArray<int>& indicesB,
|
|
|
|
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
|
|
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
|
|
const b3Vector3& sepNormalWorldSpace,
|
|
int maxContactCapacity)
|
|
{
|
|
int contactIndex = -1;
|
|
b3ConvexPolyhedronData hullA, hullB;
|
|
|
|
b3Collidable colA = hostCollidablesA[collidableIndexA];
|
|
hullA = hostConvexDataA[colA.m_shapeIndex];
|
|
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
|
|
|
b3Collidable colB = hostCollidablesB[collidableIndexB];
|
|
hullB = hostConvexDataB[colB.m_shapeIndex];
|
|
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
|
|
|
float4 contactsOut[MAX_VERTS];
|
|
int localContactCapacity = MAX_VERTS;
|
|
|
|
#ifdef _WIN32
|
|
b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
|
|
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
|
|
#endif
|
|
|
|
{
|
|
float4 worldVertsB1[MAX_VERTS];
|
|
float4 worldVertsB2[MAX_VERTS];
|
|
int capacityWorldVerts = MAX_VERTS;
|
|
|
|
float4 hostNormal = make_float4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
|
|
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
|
|
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
|
|
|
|
b3Scalar minDist = -1;
|
|
b3Scalar maxDist = 0.;
|
|
|
|
b3Transform trA, trB;
|
|
{
|
|
//B3_PROFILE("transform computation");
|
|
//trA.setIdentity();
|
|
trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
|
|
trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
|
|
|
|
//trB.setIdentity();
|
|
trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
|
|
trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
|
|
}
|
|
|
|
b3Quaternion trAorn = trA.getRotation();
|
|
b3Quaternion trBorn = trB.getRotation();
|
|
|
|
int numContactsOut = clipHullAgainstHull(hostNormal,
|
|
hostConvexDataA.at(shapeA),
|
|
hostConvexDataB.at(shapeB),
|
|
(float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
|
|
(float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
|
|
worldVertsB1, worldVertsB2, capacityWorldVerts,
|
|
minDist, maxDist,
|
|
verticesA, facesA, indicesA,
|
|
verticesB, facesB, indicesB,
|
|
|
|
contactsOut, localContactCapacity);
|
|
|
|
if (numContactsOut > 0)
|
|
{
|
|
B3_PROFILE("overlap");
|
|
|
|
float4 normalOnSurfaceB = (float4&)hostNormal;
|
|
|
|
b3Int4 contactIdx;
|
|
contactIdx.x = 0;
|
|
contactIdx.y = 1;
|
|
contactIdx.z = 2;
|
|
contactIdx.w = 3;
|
|
|
|
int numPoints = 0;
|
|
|
|
{
|
|
// B3_PROFILE("extractManifold");
|
|
numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
|
|
}
|
|
|
|
b3Assert(numPoints);
|
|
|
|
if (nContacts < maxContactCapacity)
|
|
{
|
|
contactIndex = nContacts;
|
|
globalContactOut->expand();
|
|
b3Contact4& contact = globalContactOut->at(nContacts);
|
|
contact.m_batchIdx = 0; //i;
|
|
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
|
|
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
|
|
|
|
contact.m_frictionCoeffCmp = 45874;
|
|
contact.m_restituitionCoeffCmp = 0;
|
|
|
|
// float distance = 0.f;
|
|
for (int p = 0; p < numPoints; p++)
|
|
{
|
|
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
|
|
contact.m_worldNormalOnB = normalOnSurfaceB;
|
|
}
|
|
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
|
|
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
|
|
nContacts++;
|
|
}
|
|
else
|
|
{
|
|
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
|
}
|
|
}
|
|
}
|
|
return contactIndex;
|
|
}
|
|
|
|
void computeContactPlaneConvex(int pairIndex,
|
|
int bodyIndexA, int bodyIndexB,
|
|
int collidableIndexA, int collidableIndexB,
|
|
const b3RigidBodyData* rigidBodies,
|
|
const b3Collidable* collidables,
|
|
const b3ConvexPolyhedronData* convexShapes,
|
|
const b3Vector3* convexVertices,
|
|
const int* convexIndices,
|
|
const b3GpuFace* faces,
|
|
b3Contact4* globalContactsOut,
|
|
int& nGlobalContactsOut,
|
|
int maxContactCapacity)
|
|
{
|
|
int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
|
|
const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndex];
|
|
|
|
b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
|
|
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
|
b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
|
|
|
// int numContactsOut = 0;
|
|
// int numWorldVertsB1= 0;
|
|
|
|
b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
|
|
b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z);
|
|
b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal);
|
|
float planeConstant = planeEq.w;
|
|
b3Transform convexWorldTransform;
|
|
convexWorldTransform.setIdentity();
|
|
convexWorldTransform.setOrigin(posB);
|
|
convexWorldTransform.setRotation(ornB);
|
|
b3Transform planeTransform;
|
|
planeTransform.setIdentity();
|
|
planeTransform.setOrigin(posA);
|
|
planeTransform.setRotation(ornA);
|
|
|
|
b3Transform planeInConvex;
|
|
planeInConvex = convexWorldTransform.inverse() * planeTransform;
|
|
b3Transform convexInPlane;
|
|
convexInPlane = planeTransform.inverse() * convexWorldTransform;
|
|
|
|
b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal;
|
|
float maxDot = -1e30;
|
|
int hitVertex = -1;
|
|
b3Vector3 hitVtx;
|
|
|
|
#define MAX_PLANE_CONVEX_POINTS 64
|
|
|
|
b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
|
|
int numPoints = 0;
|
|
|
|
b3Int4 contactIdx;
|
|
contactIdx.s[0] = 0;
|
|
contactIdx.s[1] = 1;
|
|
contactIdx.s[2] = 2;
|
|
contactIdx.s[3] = 3;
|
|
|
|
for (int i = 0; i < hullB->m_numVertices; i++)
|
|
{
|
|
b3Vector3 vtx = convexVertices[hullB->m_vertexOffset + i];
|
|
float curDot = vtx.dot(planeNormalInConvex);
|
|
|
|
if (curDot > maxDot)
|
|
{
|
|
hitVertex = i;
|
|
maxDot = curDot;
|
|
hitVtx = vtx;
|
|
//make sure the deepest points is always included
|
|
if (numPoints == MAX_PLANE_CONVEX_POINTS)
|
|
numPoints--;
|
|
}
|
|
|
|
if (numPoints < MAX_PLANE_CONVEX_POINTS)
|
|
{
|
|
b3Vector3 vtxWorld = convexWorldTransform * vtx;
|
|
b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld;
|
|
float dist = planeNormal.dot(vtxInPlane) - planeConstant;
|
|
if (dist < 0.f)
|
|
{
|
|
vtxWorld.w = dist;
|
|
contactPoints[numPoints] = vtxWorld;
|
|
numPoints++;
|
|
}
|
|
}
|
|
}
|
|
|
|
int numReducedPoints = 0;
|
|
|
|
numReducedPoints = numPoints;
|
|
|
|
if (numPoints > 4)
|
|
{
|
|
numReducedPoints = extractManifoldSequentialGlobal(contactPoints, numPoints, planeNormalInConvex, &contactIdx);
|
|
}
|
|
int dstIdx;
|
|
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
|
|
|
if (numReducedPoints > 0)
|
|
{
|
|
if (nGlobalContactsOut < maxContactCapacity)
|
|
{
|
|
dstIdx = nGlobalContactsOut;
|
|
nGlobalContactsOut++;
|
|
|
|
b3Contact4* c = &globalContactsOut[dstIdx];
|
|
c->m_worldNormalOnB = -planeNormalWorld;
|
|
c->setFrictionCoeff(0.7);
|
|
c->setRestituitionCoeff(0.f);
|
|
|
|
c->m_batchIdx = pairIndex;
|
|
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
|
|
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
|
|
for (int i = 0; i < numReducedPoints; i++)
|
|
{
|
|
b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
|
|
c->m_worldPosB[i] = pOnB1;
|
|
}
|
|
c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
|
|
} //if (dstIdx < numPairs)
|
|
}
|
|
|
|
// printf("computeContactPlaneConvex\n");
|
|
}
|
|
|
|
B3_FORCE_INLINE b3Vector3 MyUnQuantize(const unsigned short* vecIn, const b3Vector3& quantization, const b3Vector3& bvhAabbMin)
|
|
{
|
|
b3Vector3 vecOut;
|
|
vecOut.setValue(
|
|
(b3Scalar)(vecIn[0]) / (quantization.x),
|
|
(b3Scalar)(vecIn[1]) / (quantization.y),
|
|
(b3Scalar)(vecIn[2]) / (quantization.z));
|
|
vecOut += bvhAabbMin;
|
|
return vecOut;
|
|
}
|
|
|
|
void traverseTreeTree()
|
|
{
|
|
}
|
|
|
|
#include "Bullet3Common/shared/b3Mat3x3.h"
|
|
|
|
int numAabbChecks = 0;
|
|
int maxNumAabbChecks = 0;
|
|
int maxDepth = 0;
|
|
|
|
// work-in-progress
|
|
__kernel void findCompoundPairsKernel(
|
|
int pairIndex,
|
|
int bodyIndexA,
|
|
int bodyIndexB,
|
|
int collidableIndexA,
|
|
int collidableIndexB,
|
|
__global const b3RigidBodyData* rigidBodies,
|
|
__global const b3Collidable* collidables,
|
|
__global const b3ConvexPolyhedronData* convexShapes,
|
|
__global const b3AlignedObjectArray<b3Float4>& vertices,
|
|
__global const b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace,
|
|
__global const b3AlignedObjectArray<b3Aabb>& aabbsLocalSpace,
|
|
__global const b3GpuChildShape* gpuChildShapes,
|
|
__global b3Int4* gpuCompoundPairsOut,
|
|
__global int* numCompoundPairsOut,
|
|
int maxNumCompoundPairsCapacity,
|
|
b3AlignedObjectArray<b3QuantizedBvhNode>& treeNodesCPU,
|
|
b3AlignedObjectArray<b3BvhSubtreeInfo>& subTreesCPU,
|
|
b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU)
|
|
{
|
|
numAabbChecks = 0;
|
|
maxNumAabbChecks = 0;
|
|
// int i = pairIndex;
|
|
{
|
|
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
|
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
|
|
|
//once the broadphase avoids static-static pairs, we can remove this test
|
|
if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if ((collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) && (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
|
|
{
|
|
int bvhA = collidables[collidableIndexA].m_compoundBvhIndex;
|
|
int bvhB = collidables[collidableIndexB].m_compoundBvhIndex;
|
|
int numSubTreesA = bvhInfoCPU[bvhA].m_numSubTrees;
|
|
int subTreesOffsetA = bvhInfoCPU[bvhA].m_subTreeOffset;
|
|
int subTreesOffsetB = bvhInfoCPU[bvhB].m_subTreeOffset;
|
|
|
|
int numSubTreesB = bvhInfoCPU[bvhB].m_numSubTrees;
|
|
|
|
float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
|
|
|
|
b3Transform transA;
|
|
transA.setIdentity();
|
|
transA.setOrigin(posA);
|
|
transA.setRotation(ornA);
|
|
|
|
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
|
|
float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
|
|
b3Transform transB;
|
|
transB.setIdentity();
|
|
transB.setOrigin(posB);
|
|
transB.setRotation(ornB);
|
|
|
|
for (int p = 0; p < numSubTreesA; p++)
|
|
{
|
|
b3BvhSubtreeInfo subtreeA = subTreesCPU[subTreesOffsetA + p];
|
|
//bvhInfoCPU[bvhA].m_quantization
|
|
b3Vector3 treeAminLocal = MyUnQuantize(subtreeA.m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
|
|
b3Vector3 treeAmaxLocal = MyUnQuantize(subtreeA.m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
|
|
|
|
b3Vector3 aabbAMinOut, aabbAMaxOut;
|
|
float margin = 0.f;
|
|
b3TransformAabb2(treeAminLocal, treeAmaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);
|
|
|
|
for (int q = 0; q < numSubTreesB; q++)
|
|
{
|
|
b3BvhSubtreeInfo subtreeB = subTreesCPU[subTreesOffsetB + q];
|
|
|
|
b3Vector3 treeBminLocal = MyUnQuantize(subtreeB.m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
|
|
b3Vector3 treeBmaxLocal = MyUnQuantize(subtreeB.m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
|
|
|
|
b3Vector3 aabbBMinOut, aabbBMaxOut;
|
|
float margin = 0.f;
|
|
b3TransformAabb2(treeBminLocal, treeBmaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);
|
|
|
|
numAabbChecks = 0;
|
|
bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
|
|
if (aabbOverlap)
|
|
{
|
|
int startNodeIndexA = subtreeA.m_rootNodeIndex + bvhInfoCPU[bvhA].m_nodeOffset;
|
|
// int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize;
|
|
|
|
int startNodeIndexB = subtreeB.m_rootNodeIndex + bvhInfoCPU[bvhB].m_nodeOffset;
|
|
// int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize;
|
|
|
|
b3AlignedObjectArray<b3Int2> nodeStack;
|
|
b3Int2 node0;
|
|
node0.x = startNodeIndexA;
|
|
node0.y = startNodeIndexB;
|
|
|
|
int maxStackDepth = 1024;
|
|
nodeStack.resize(maxStackDepth);
|
|
int depth = 0;
|
|
nodeStack[depth++] = node0;
|
|
|
|
do
|
|
{
|
|
if (depth > maxDepth)
|
|
{
|
|
maxDepth = depth;
|
|
printf("maxDepth=%d\n", maxDepth);
|
|
}
|
|
b3Int2 node = nodeStack[--depth];
|
|
|
|
b3Vector3 aMinLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMin, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
|
|
b3Vector3 aMaxLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMax, bvhInfoCPU[bvhA].m_quantization, bvhInfoCPU[bvhA].m_aabbMin);
|
|
|
|
b3Vector3 bMinLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMin, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
|
|
b3Vector3 bMaxLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMax, bvhInfoCPU[bvhB].m_quantization, bvhInfoCPU[bvhB].m_aabbMin);
|
|
|
|
float margin = 0.f;
|
|
b3Vector3 aabbAMinOut, aabbAMaxOut;
|
|
b3TransformAabb2(aMinLocal, aMaxLocal, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);
|
|
|
|
b3Vector3 aabbBMinOut, aabbBMaxOut;
|
|
b3TransformAabb2(bMinLocal, bMaxLocal, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);
|
|
|
|
numAabbChecks++;
|
|
bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
|
|
if (nodeOverlap)
|
|
{
|
|
bool isLeafA = treeNodesCPU[node.x].isLeafNode();
|
|
bool isLeafB = treeNodesCPU[node.y].isLeafNode();
|
|
bool isInternalA = !isLeafA;
|
|
bool isInternalB = !isLeafB;
|
|
|
|
//fail, even though it might hit two leaf nodes
|
|
if (depth + 4 > maxStackDepth && !(isLeafA && isLeafB))
|
|
{
|
|
b3Error("Error: traversal exceeded maxStackDepth\n");
|
|
continue;
|
|
}
|
|
|
|
if (isInternalA)
|
|
{
|
|
int nodeAleftChild = node.x + 1;
|
|
bool isNodeALeftChildLeaf = treeNodesCPU[node.x + 1].isLeafNode();
|
|
int nodeArightChild = isNodeALeftChildLeaf ? node.x + 2 : node.x + 1 + treeNodesCPU[node.x + 1].getEscapeIndex();
|
|
|
|
if (isInternalB)
|
|
{
|
|
int nodeBleftChild = node.y + 1;
|
|
bool isNodeBLeftChildLeaf = treeNodesCPU[node.y + 1].isLeafNode();
|
|
int nodeBrightChild = isNodeBLeftChildLeaf ? node.y + 2 : node.y + 1 + treeNodesCPU[node.y + 1].getEscapeIndex();
|
|
|
|
nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild);
|
|
nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild);
|
|
nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild);
|
|
nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild);
|
|
}
|
|
else
|
|
{
|
|
nodeStack[depth++] = b3MakeInt2(nodeAleftChild, node.y);
|
|
nodeStack[depth++] = b3MakeInt2(nodeArightChild, node.y);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (isInternalB)
|
|
{
|
|
int nodeBleftChild = node.y + 1;
|
|
bool isNodeBLeftChildLeaf = treeNodesCPU[node.y + 1].isLeafNode();
|
|
int nodeBrightChild = isNodeBLeftChildLeaf ? node.y + 2 : node.y + 1 + treeNodesCPU[node.y + 1].getEscapeIndex();
|
|
nodeStack[depth++] = b3MakeInt2(node.x, nodeBleftChild);
|
|
nodeStack[depth++] = b3MakeInt2(node.x, nodeBrightChild);
|
|
}
|
|
else
|
|
{
|
|
int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
|
|
if (compoundPairIdx < maxNumCompoundPairsCapacity)
|
|
{
|
|
int childShapeIndexA = treeNodesCPU[node.x].getTriangleIndex();
|
|
int childShapeIndexB = treeNodesCPU[node.y].getTriangleIndex();
|
|
gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} while (depth);
|
|
maxNumAabbChecks = b3Max(numAabbChecks, maxNumAabbChecks);
|
|
}
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
if ((collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) || (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
|
|
{
|
|
if (collidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
int numChildrenA = collidables[collidableIndexA].m_numChildShapes;
|
|
for (int c = 0; c < numChildrenA; c++)
|
|
{
|
|
int childShapeIndexA = collidables[collidableIndexA].m_shapeIndex + c;
|
|
int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
|
|
|
|
float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
|
|
float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
|
|
b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
|
|
float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
|
|
b3Quat newOrnA = b3QuatMul(ornA, childOrnA);
|
|
|
|
b3Aabb aabbA = aabbsLocalSpace[childColIndexA];
|
|
|
|
b3Transform transA;
|
|
transA.setIdentity();
|
|
transA.setOrigin(newPosA);
|
|
transA.setRotation(newOrnA);
|
|
b3Scalar margin = 0.0f;
|
|
|
|
b3Vector3 aabbAMinOut, aabbAMaxOut;
|
|
|
|
b3TransformAabb2((const b3Float4&)aabbA.m_min, (const b3Float4&)aabbA.m_max, margin, transA.getOrigin(), transA.getRotation(), &aabbAMinOut, &aabbAMaxOut);
|
|
|
|
if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
|
for (int b = 0; b < numChildrenB; b++)
|
|
{
|
|
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
|
|
int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
|
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
|
|
float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
|
b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
|
float4 newPosB = transform(&childPosB, &posB, &ornB);
|
|
b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
|
|
|
|
b3Aabb aabbB = aabbsLocalSpace[childColIndexB];
|
|
|
|
b3Transform transB;
|
|
transB.setIdentity();
|
|
transB.setOrigin(newPosB);
|
|
transB.setRotation(newOrnB);
|
|
|
|
b3Vector3 aabbBMinOut, aabbBMaxOut;
|
|
b3TransformAabb2((const b3Float4&)aabbB.m_min, (const b3Float4&)aabbB.m_max, margin, transB.getOrigin(), transB.getRotation(), &aabbBMinOut, &aabbBMaxOut);
|
|
|
|
numAabbChecks++;
|
|
bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut, aabbAMaxOut, aabbBMinOut, aabbBMaxOut);
|
|
if (aabbOverlap)
|
|
{
|
|
/*
|
|
int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
|
float dmin = FLT_MAX;
|
|
float4 posA = newPosA;
|
|
posA.w = 0.f;
|
|
float4 posB = newPosB;
|
|
posB.w = 0.f;
|
|
float4 c0local = convexShapes[shapeIndexA].m_localCenter;
|
|
b3Quat ornA = newOrnA;
|
|
float4 c0 = transform(&c0local, &posA, &ornA);
|
|
float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
|
b3Quat ornB =newOrnB;
|
|
float4 c1 = transform(&c1local,&posB,&ornB);
|
|
const float4 DeltaC2 = c0 - c1;
|
|
*/
|
|
{ //
|
|
int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
|
|
if (compoundPairIdx < maxNumCompoundPairsCapacity)
|
|
{
|
|
gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, childShapeIndexB);
|
|
}
|
|
} //
|
|
} //fi(1)
|
|
} //for (int b=0
|
|
} //if (collidables[collidableIndexB].
|
|
else //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
if (1)
|
|
{
|
|
// int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
|
// float dmin = FLT_MAX;
|
|
float4 posA = newPosA;
|
|
posA.w = 0.f;
|
|
float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
posB.w = 0.f;
|
|
float4 c0local = convexShapes[shapeIndexA].m_localCenter;
|
|
b3Quat ornA = newOrnA;
|
|
float4 c0;
|
|
c0 = transform(&c0local, &posA, &ornA);
|
|
float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
|
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
|
|
float4 c1;
|
|
c1 = transform(&c1local, &posB, &ornB);
|
|
// const float4 DeltaC2 = c0 - c1;
|
|
|
|
{
|
|
int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
|
|
if (compoundPairIdx < maxNumCompoundPairsCapacity)
|
|
{
|
|
gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, childShapeIndexA, -1);
|
|
} //if (compoundPairIdx<maxNumCompoundPairsCapacity)
|
|
} //
|
|
} //fi (1)
|
|
} //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
} //for (int b=0;b<numChildrenB;b++)
|
|
return;
|
|
} //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
if ((collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH) && (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS))
|
|
{
|
|
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
|
for (int b = 0; b < numChildrenB; b++)
|
|
{
|
|
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
|
|
int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
|
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
|
|
float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
|
b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
|
float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
|
|
b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
|
|
|
|
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
|
|
|
//////////////////////////////////////
|
|
|
|
if (1)
|
|
{
|
|
// int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
|
// float dmin = FLT_MAX;
|
|
float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
posA.w = 0.f;
|
|
float4 posB = newPosB;
|
|
posB.w = 0.f;
|
|
float4 c0local = convexShapes[shapeIndexA].m_localCenter;
|
|
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
|
|
float4 c0;
|
|
c0 = transform(&c0local, &posA, &ornA);
|
|
float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
|
b3Quat ornB = newOrnB;
|
|
float4 c1;
|
|
c1 = transform(&c1local, &posB, &ornB);
|
|
// const float4 DeltaC2 = c0 - c1;
|
|
{ //
|
|
int compoundPairIdx = b3AtomicInc(numCompoundPairsOut);
|
|
if (compoundPairIdx < maxNumCompoundPairsCapacity)
|
|
{
|
|
gpuCompoundPairsOut[compoundPairIdx] = b3MakeInt4(bodyIndexA, bodyIndexB, -1, childShapeIndexB);
|
|
} //fi (compoundPairIdx<maxNumCompoundPairsCapacity)
|
|
} //
|
|
} //fi (1)
|
|
} //for (int b=0;b<numChildrenB;b++)
|
|
return;
|
|
} //if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
return;
|
|
} //fi ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) ||(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))
|
|
} //i<numPairs
|
|
}
|
|
|
|
__kernel void processCompoundPairsKernel(__global const b3Int4* gpuCompoundPairs,
|
|
__global const b3RigidBodyData* rigidBodies,
|
|
__global const b3Collidable* collidables,
|
|
__global const b3ConvexPolyhedronData* convexShapes,
|
|
__global const b3AlignedObjectArray<b3Float4>& vertices,
|
|
__global const b3AlignedObjectArray<b3Float4>& uniqueEdges,
|
|
__global const b3AlignedObjectArray<b3GpuFace>& faces,
|
|
__global const b3AlignedObjectArray<int>& indices,
|
|
__global b3Aabb* aabbs,
|
|
__global const b3GpuChildShape* gpuChildShapes,
|
|
__global b3AlignedObjectArray<b3Float4>& gpuCompoundSepNormalsOut,
|
|
__global b3AlignedObjectArray<int>& gpuHasCompoundSepNormalsOut,
|
|
int numCompoundPairs,
|
|
int i)
|
|
{
|
|
// int i = get_global_id(0);
|
|
if (i < numCompoundPairs)
|
|
{
|
|
int bodyIndexA = gpuCompoundPairs[i].x;
|
|
int bodyIndexB = gpuCompoundPairs[i].y;
|
|
|
|
int childShapeIndexA = gpuCompoundPairs[i].z;
|
|
int childShapeIndexB = gpuCompoundPairs[i].w;
|
|
|
|
int collidableIndexA = -1;
|
|
int collidableIndexB = -1;
|
|
|
|
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
|
|
float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
|
|
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
|
|
float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
|
|
if (childShapeIndexA >= 0)
|
|
{
|
|
collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
|
|
float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
|
|
b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
|
|
float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
|
|
b3Quat newOrnA = b3QuatMul(ornA, childOrnA);
|
|
posA = newPosA;
|
|
ornA = newOrnA;
|
|
}
|
|
else
|
|
{
|
|
collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
|
}
|
|
|
|
if (childShapeIndexB >= 0)
|
|
{
|
|
collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
|
float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
|
b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
|
float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
|
|
b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
|
|
posB = newPosB;
|
|
ornB = newOrnB;
|
|
}
|
|
else
|
|
{
|
|
collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
|
}
|
|
|
|
gpuHasCompoundSepNormalsOut[i] = 0;
|
|
|
|
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
|
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
|
|
|
int shapeTypeA = collidables[collidableIndexA].m_shapeType;
|
|
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
|
|
|
if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))
|
|
{
|
|
return;
|
|
}
|
|
|
|
int hasSeparatingAxis = 5;
|
|
|
|
// int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
|
float dmin = FLT_MAX;
|
|
posA.w = 0.f;
|
|
posB.w = 0.f;
|
|
float4 c0local = convexShapes[shapeIndexA].m_localCenter;
|
|
float4 c0 = transform(&c0local, &posA, &ornA);
|
|
float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
|
float4 c1 = transform(&c1local, &posB, &ornB);
|
|
const float4 DeltaC2 = c0 - c1;
|
|
float4 sepNormal = make_float4(1, 0, 0, 0);
|
|
// bool sepA = findSeparatingAxis( convexShapes[shapeIndexA], convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin);
|
|
bool sepA = findSeparatingAxis(convexShapes[shapeIndexA], convexShapes[shapeIndexB], posA, ornA, posB, ornB, vertices, uniqueEdges, faces, indices, vertices, uniqueEdges, faces, indices, sepNormal); //,&dmin);
|
|
|
|
hasSeparatingAxis = 4;
|
|
if (!sepA)
|
|
{
|
|
hasSeparatingAxis = 0;
|
|
}
|
|
else
|
|
{
|
|
bool sepB = findSeparatingAxis(convexShapes[shapeIndexB], convexShapes[shapeIndexA], posB, ornB, posA, ornA, vertices, uniqueEdges, faces, indices, vertices, uniqueEdges, faces, indices, sepNormal); //,&dmin);
|
|
|
|
if (!sepB)
|
|
{
|
|
hasSeparatingAxis = 0;
|
|
}
|
|
else //(!sepB)
|
|
{
|
|
bool sepEE = findSeparatingAxisEdgeEdge(&convexShapes[shapeIndexA], &convexShapes[shapeIndexB], posA, ornA, posB, ornB, DeltaC2, vertices, uniqueEdges, faces, indices, &sepNormal, &dmin);
|
|
if (sepEE)
|
|
{
|
|
gpuCompoundSepNormalsOut[i] = sepNormal; //fastNormalize4(sepNormal);
|
|
gpuHasCompoundSepNormalsOut[i] = 1;
|
|
} //sepEE
|
|
} //(!sepB)
|
|
} //(!sepA)
|
|
}
|
|
}
|
|
|
|
__kernel void clipCompoundsHullHullKernel(__global const b3Int4* gpuCompoundPairs,
|
|
__global const b3RigidBodyData* rigidBodies,
|
|
__global const b3Collidable* collidables,
|
|
__global const b3ConvexPolyhedronData* convexShapes,
|
|
__global const b3AlignedObjectArray<b3Float4>& vertices,
|
|
__global const b3AlignedObjectArray<b3Float4>& uniqueEdges,
|
|
__global const b3AlignedObjectArray<b3GpuFace>& faces,
|
|
__global const b3AlignedObjectArray<int>& indices,
|
|
__global const b3GpuChildShape* gpuChildShapes,
|
|
__global const b3AlignedObjectArray<b3Float4>& gpuCompoundSepNormalsOut,
|
|
__global const b3AlignedObjectArray<int>& gpuHasCompoundSepNormalsOut,
|
|
__global struct b3Contact4Data* globalContactsOut,
|
|
int* nGlobalContactsOut,
|
|
int numCompoundPairs, int maxContactCapacity, int i)
|
|
{
|
|
// int i = get_global_id(0);
|
|
int pairIndex = i;
|
|
|
|
float4 worldVertsB1[64];
|
|
float4 worldVertsB2[64];
|
|
int capacityWorldVerts = 64;
|
|
|
|
float4 localContactsOut[64];
|
|
int localContactCapacity = 64;
|
|
|
|
float minDist = -1e30f;
|
|
float maxDist = 0.0f;
|
|
|
|
if (i < numCompoundPairs)
|
|
{
|
|
if (gpuHasCompoundSepNormalsOut[i])
|
|
{
|
|
int bodyIndexA = gpuCompoundPairs[i].x;
|
|
int bodyIndexB = gpuCompoundPairs[i].y;
|
|
|
|
int childShapeIndexA = gpuCompoundPairs[i].z;
|
|
int childShapeIndexB = gpuCompoundPairs[i].w;
|
|
|
|
int collidableIndexA = -1;
|
|
int collidableIndexB = -1;
|
|
|
|
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
|
|
float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
|
|
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
|
|
float4 posB = rigidBodies[bodyIndexB].m_pos;
|
|
|
|
if (childShapeIndexA >= 0)
|
|
{
|
|
collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
|
|
float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
|
|
b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
|
|
float4 newPosA = b3QuatRotate(ornA, childPosA) + posA;
|
|
b3Quat newOrnA = b3QuatMul(ornA, childOrnA);
|
|
posA = newPosA;
|
|
ornA = newOrnA;
|
|
}
|
|
else
|
|
{
|
|
collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
|
}
|
|
|
|
if (childShapeIndexB >= 0)
|
|
{
|
|
collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
|
float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
|
b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
|
float4 newPosB = b3QuatRotate(ornB, childPosB) + posB;
|
|
b3Quat newOrnB = b3QuatMul(ornB, childOrnB);
|
|
posB = newPosB;
|
|
ornB = newOrnB;
|
|
}
|
|
else
|
|
{
|
|
collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
|
}
|
|
|
|
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
|
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
|
|
|
int numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i],
|
|
convexShapes[shapeIndexA], convexShapes[shapeIndexB],
|
|
posA, ornA,
|
|
posB, ornB,
|
|
worldVertsB1, worldVertsB2, capacityWorldVerts,
|
|
minDist, maxDist,
|
|
vertices, faces, indices,
|
|
vertices, faces, indices,
|
|
localContactsOut, localContactCapacity);
|
|
|
|
if (numLocalContactsOut > 0)
|
|
{
|
|
float4 normal = -gpuCompoundSepNormalsOut[i];
|
|
int nPoints = numLocalContactsOut;
|
|
float4* pointsIn = localContactsOut;
|
|
b3Int4 contactIdx; // = {-1,-1,-1,-1};
|
|
|
|
contactIdx.s[0] = 0;
|
|
contactIdx.s[1] = 1;
|
|
contactIdx.s[2] = 2;
|
|
contactIdx.s[3] = 3;
|
|
|
|
int nReducedContacts = extractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
|
|
|
|
int dstIdx;
|
|
dstIdx = b3AtomicInc(nGlobalContactsOut);
|
|
if ((dstIdx + nReducedContacts) < maxContactCapacity)
|
|
{
|
|
__global struct b3Contact4Data* c = globalContactsOut + dstIdx;
|
|
c->m_worldNormalOnB = -normal;
|
|
c->m_restituitionCoeffCmp = (0.f * 0xffff);
|
|
c->m_frictionCoeffCmp = (0.7f * 0xffff);
|
|
c->m_batchIdx = pairIndex;
|
|
int bodyA = gpuCompoundPairs[pairIndex].x;
|
|
int bodyB = gpuCompoundPairs[pairIndex].y;
|
|
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
|
|
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
|
|
c->m_childIndexA = childShapeIndexA;
|
|
c->m_childIndexB = childShapeIndexB;
|
|
for (int i = 0; i < nReducedContacts; i++)
|
|
{
|
|
c->m_worldPosB[i] = pointsIn[contactIdx.s[i]];
|
|
}
|
|
b3Contact4Data_setNumPoints(c, nReducedContacts);
|
|
}
|
|
|
|
} // if (numContactsOut>0)
|
|
} // if (gpuHasCompoundSepNormalsOut[i])
|
|
} // if (i<numCompoundPairs)
|
|
}
|
|
|
|
void computeContactCompoundCompound(int pairIndex,
|
|
int bodyIndexA, int bodyIndexB,
|
|
int collidableIndexA, int collidableIndexB,
|
|
const b3RigidBodyData* rigidBodies,
|
|
const b3Collidable* collidables,
|
|
const b3ConvexPolyhedronData* convexShapes,
|
|
const b3GpuChildShape* cpuChildShapes,
|
|
const b3AlignedObjectArray<b3Aabb>& hostAabbsWorldSpace,
|
|
const b3AlignedObjectArray<b3Aabb>& hostAabbsLocalSpace,
|
|
|
|
const b3AlignedObjectArray<b3Vector3>& convexVertices,
|
|
const b3AlignedObjectArray<b3Vector3>& hostUniqueEdges,
|
|
const b3AlignedObjectArray<int>& convexIndices,
|
|
const b3AlignedObjectArray<b3GpuFace>& faces,
|
|
|
|
b3Contact4* globalContactsOut,
|
|
int& nGlobalContactsOut,
|
|
int maxContactCapacity,
|
|
b3AlignedObjectArray<b3QuantizedBvhNode>& treeNodesCPU,
|
|
b3AlignedObjectArray<b3BvhSubtreeInfo>& subTreesCPU,
|
|
b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU)
|
|
{
|
|
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
|
b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);
|
|
|
|
b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
|
|
int numCompoundPairsOut = 0;
|
|
int maxNumCompoundPairsCapacity = 8192; //1024;
|
|
cpuCompoundPairsOut.resize(maxNumCompoundPairsCapacity);
|
|
|
|
// work-in-progress
|
|
findCompoundPairsKernel(
|
|
pairIndex,
|
|
bodyIndexA, bodyIndexB,
|
|
collidableIndexA, collidableIndexB,
|
|
rigidBodies,
|
|
collidables,
|
|
convexShapes,
|
|
convexVertices,
|
|
hostAabbsWorldSpace,
|
|
hostAabbsLocalSpace,
|
|
cpuChildShapes,
|
|
&cpuCompoundPairsOut[0],
|
|
&numCompoundPairsOut,
|
|
maxNumCompoundPairsCapacity,
|
|
treeNodesCPU,
|
|
subTreesCPU,
|
|
bvhInfoCPU);
|
|
|
|
printf("maxNumAabbChecks=%d\n", maxNumAabbChecks);
|
|
if (numCompoundPairsOut > maxNumCompoundPairsCapacity)
|
|
{
|
|
b3Error("numCompoundPairsOut exceeded maxNumCompoundPairsCapacity (%d)\n", maxNumCompoundPairsCapacity);
|
|
numCompoundPairsOut = maxNumCompoundPairsCapacity;
|
|
}
|
|
b3AlignedObjectArray<b3Float4> cpuCompoundSepNormalsOut;
|
|
b3AlignedObjectArray<int> cpuHasCompoundSepNormalsOut;
|
|
cpuCompoundSepNormalsOut.resize(numCompoundPairsOut);
|
|
cpuHasCompoundSepNormalsOut.resize(numCompoundPairsOut);
|
|
|
|
for (int i = 0; i < numCompoundPairsOut; i++)
|
|
{
|
|
processCompoundPairsKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, 0, cpuChildShapes,
|
|
cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, numCompoundPairsOut, i);
|
|
}
|
|
|
|
for (int i = 0; i < numCompoundPairsOut; i++)
|
|
{
|
|
clipCompoundsHullHullKernel(&cpuCompoundPairsOut[0], rigidBodies, collidables, convexShapes, convexVertices, hostUniqueEdges, faces, convexIndices, cpuChildShapes,
|
|
cpuCompoundSepNormalsOut, cpuHasCompoundSepNormalsOut, globalContactsOut, &nGlobalContactsOut, numCompoundPairsOut, maxContactCapacity, i);
|
|
}
|
|
/*
|
|
int childColIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
|
|
|
|
float4 posA = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
|
|
float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
|
|
b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
|
|
float4 newPosA = b3QuatRotate(ornA,childPosA)+posA;
|
|
b3Quat newOrnA = b3QuatMul(ornA,childOrnA);
|
|
|
|
int shapeIndexA = collidables[childColIndexA].m_shapeIndex;
|
|
|
|
|
|
bool foundSepAxis = findSeparatingAxis(hullA,hullB,
|
|
posA,
|
|
ornA,
|
|
posB,
|
|
ornB,
|
|
|
|
convexVertices,uniqueEdges,faces,convexIndices,
|
|
convexVertices,uniqueEdges,faces,convexIndices,
|
|
|
|
sepNormalWorldSpace
|
|
);
|
|
*/
|
|
|
|
/*
|
|
if (foundSepAxis)
|
|
{
|
|
|
|
|
|
contactIndex = clipHullHullSingle(
|
|
bodyIndexA, bodyIndexB,
|
|
posA,ornA,
|
|
posB,ornB,
|
|
collidableIndexA, collidableIndexB,
|
|
&rigidBodies,
|
|
&globalContactsOut,
|
|
nGlobalContactsOut,
|
|
|
|
convexShapes,
|
|
convexShapes,
|
|
|
|
convexVertices,
|
|
uniqueEdges,
|
|
faces,
|
|
convexIndices,
|
|
|
|
convexVertices,
|
|
uniqueEdges,
|
|
faces,
|
|
convexIndices,
|
|
|
|
collidables,
|
|
collidables,
|
|
sepNormalWorldSpace,
|
|
maxContactCapacity);
|
|
|
|
}
|
|
*/
|
|
|
|
// return contactIndex;
|
|
|
|
/*
|
|
|
|
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
|
for (int c=0;c<numChildrenB;c++)
|
|
{
|
|
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+c;
|
|
int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex;
|
|
|
|
float4 rootPosB = rigidBodies[bodyIndexB].m_pos;
|
|
b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat;
|
|
b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition;
|
|
b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation;
|
|
float4 posB = b3QuatRotate(rootOrnB,childPosB)+rootPosB;
|
|
b3Quaternion ornB = b3QuatMul(rootOrnB,childOrnB);//b3QuatMul(ornB,childOrnB);
|
|
|
|
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
|
|
|
const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];
|
|
|
|
}
|
|
*/
|
|
}
|
|
|
|
void computeContactPlaneCompound(int pairIndex,
|
|
int bodyIndexA, int bodyIndexB,
|
|
int collidableIndexA, int collidableIndexB,
|
|
const b3RigidBodyData* rigidBodies,
|
|
const b3Collidable* collidables,
|
|
const b3ConvexPolyhedronData* convexShapes,
|
|
const b3GpuChildShape* cpuChildShapes,
|
|
const b3Vector3* convexVertices,
|
|
const int* convexIndices,
|
|
const b3GpuFace* faces,
|
|
|
|
b3Contact4* globalContactsOut,
|
|
int& nGlobalContactsOut,
|
|
int maxContactCapacity)
|
|
{
|
|
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
|
b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);
|
|
|
|
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
|
for (int c = 0; c < numChildrenB; c++)
|
|
{
|
|
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + c;
|
|
int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex;
|
|
|
|
float4 rootPosB = rigidBodies[bodyIndexB].m_pos;
|
|
b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat;
|
|
b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition;
|
|
b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation;
|
|
float4 posB = b3QuatRotate(rootOrnB, childPosB) + rootPosB;
|
|
b3Quaternion ornB = rootOrnB * childOrnB; //b3QuatMul(ornB,childOrnB);
|
|
|
|
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
|
|
|
const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];
|
|
|
|
b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
|
|
|
// int numContactsOut = 0;
|
|
// int numWorldVertsB1= 0;
|
|
|
|
b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
|
|
b3Vector3 planeNormal = b3MakeVector3(planeEq.x, planeEq.y, planeEq.z);
|
|
b3Vector3 planeNormalWorld = b3QuatRotate(ornA, planeNormal);
|
|
float planeConstant = planeEq.w;
|
|
b3Transform convexWorldTransform;
|
|
convexWorldTransform.setIdentity();
|
|
convexWorldTransform.setOrigin(posB);
|
|
convexWorldTransform.setRotation(ornB);
|
|
b3Transform planeTransform;
|
|
planeTransform.setIdentity();
|
|
planeTransform.setOrigin(posA);
|
|
planeTransform.setRotation(ornA);
|
|
|
|
b3Transform planeInConvex;
|
|
planeInConvex = convexWorldTransform.inverse() * planeTransform;
|
|
b3Transform convexInPlane;
|
|
convexInPlane = planeTransform.inverse() * convexWorldTransform;
|
|
|
|
b3Vector3 planeNormalInConvex = planeInConvex.getBasis() * -planeNormal;
|
|
float maxDot = -1e30;
|
|
int hitVertex = -1;
|
|
b3Vector3 hitVtx;
|
|
|
|
#define MAX_PLANE_CONVEX_POINTS 64
|
|
|
|
b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
|
|
int numPoints = 0;
|
|
|
|
b3Int4 contactIdx;
|
|
contactIdx.s[0] = 0;
|
|
contactIdx.s[1] = 1;
|
|
contactIdx.s[2] = 2;
|
|
contactIdx.s[3] = 3;
|
|
|
|
for (int i = 0; i < hullB->m_numVertices; i++)
|
|
{
|
|
b3Vector3 vtx = convexVertices[hullB->m_vertexOffset + i];
|
|
float curDot = vtx.dot(planeNormalInConvex);
|
|
|
|
if (curDot > maxDot)
|
|
{
|
|
hitVertex = i;
|
|
maxDot = curDot;
|
|
hitVtx = vtx;
|
|
//make sure the deepest points is always included
|
|
if (numPoints == MAX_PLANE_CONVEX_POINTS)
|
|
numPoints--;
|
|
}
|
|
|
|
if (numPoints < MAX_PLANE_CONVEX_POINTS)
|
|
{
|
|
b3Vector3 vtxWorld = convexWorldTransform * vtx;
|
|
b3Vector3 vtxInPlane = planeTransform.inverse() * vtxWorld;
|
|
float dist = planeNormal.dot(vtxInPlane) - planeConstant;
|
|
if (dist < 0.f)
|
|
{
|
|
vtxWorld.w = dist;
|
|
contactPoints[numPoints] = vtxWorld;
|
|
numPoints++;
|
|
}
|
|
}
|
|
}
|
|
|
|
int numReducedPoints = 0;
|
|
|
|
numReducedPoints = numPoints;
|
|
|
|
if (numPoints > 4)
|
|
{
|
|
numReducedPoints = extractManifoldSequentialGlobal(contactPoints, numPoints, planeNormalInConvex, &contactIdx);
|
|
}
|
|
int dstIdx;
|
|
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
|
|
|
if (numReducedPoints > 0)
|
|
{
|
|
if (nGlobalContactsOut < maxContactCapacity)
|
|
{
|
|
dstIdx = nGlobalContactsOut;
|
|
nGlobalContactsOut++;
|
|
|
|
b3Contact4* c = &globalContactsOut[dstIdx];
|
|
c->m_worldNormalOnB = -planeNormalWorld;
|
|
c->setFrictionCoeff(0.7);
|
|
c->setRestituitionCoeff(0.f);
|
|
|
|
c->m_batchIdx = pairIndex;
|
|
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
|
|
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
|
|
for (int i = 0; i < numReducedPoints; i++)
|
|
{
|
|
b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
|
|
c->m_worldPosB[i] = pOnB1;
|
|
}
|
|
c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
|
|
} //if (dstIdx < numPairs)
|
|
}
|
|
}
|
|
}
|
|
|
|
void computeContactSphereConvex(int pairIndex,
|
|
int bodyIndexA, int bodyIndexB,
|
|
int collidableIndexA, int collidableIndexB,
|
|
const b3RigidBodyData* rigidBodies,
|
|
const b3Collidable* collidables,
|
|
const b3ConvexPolyhedronData* convexShapes,
|
|
const b3Vector3* convexVertices,
|
|
const int* convexIndices,
|
|
const b3GpuFace* faces,
|
|
b3Contact4* globalContactsOut,
|
|
int& nGlobalContactsOut,
|
|
int maxContactCapacity)
|
|
{
|
|
float radius = collidables[collidableIndexA].m_radius;
|
|
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
|
|
|
|
float4 pos = rigidBodies[bodyIndexB].m_pos;
|
|
|
|
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
|
|
|
|
b3Transform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(pos);
|
|
tr.setRotation(quat);
|
|
b3Transform trInv = tr.inverse();
|
|
|
|
float4 spherePos = trInv(spherePos1);
|
|
|
|
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
|
|
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
|
int numFaces = convexShapes[shapeIndex].m_numFaces;
|
|
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
|
|
// float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
|
|
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
|
|
bool bCollide = true;
|
|
int region = -1;
|
|
float4 localHitNormal;
|
|
for (int f = 0; f < numFaces; f++)
|
|
{
|
|
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
|
|
float4 planeEqn;
|
|
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
|
|
float4 n1 = localPlaneNormal; //quatRotate(quat,localPlaneNormal);
|
|
planeEqn = n1;
|
|
planeEqn[3] = face.m_plane.w;
|
|
|
|
float4 pntReturn;
|
|
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
|
|
|
|
if (dist > radius)
|
|
{
|
|
bCollide = false;
|
|
break;
|
|
}
|
|
|
|
if (dist > 0)
|
|
{
|
|
//might hit an edge or vertex
|
|
b3Vector3 out;
|
|
|
|
bool isInPoly = IsPointInPolygon(spherePos,
|
|
&face,
|
|
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
|
|
convexIndices,
|
|
&out);
|
|
if (isInPoly)
|
|
{
|
|
if (dist > minDist)
|
|
{
|
|
minDist = dist;
|
|
closestPnt = pntReturn;
|
|
localHitNormal = planeEqn;
|
|
region = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3Vector3 tmp = spherePos - out;
|
|
b3Scalar l2 = tmp.length2();
|
|
if (l2 < radius * radius)
|
|
{
|
|
dist = b3Sqrt(l2);
|
|
if (dist > minDist)
|
|
{
|
|
minDist = dist;
|
|
closestPnt = out;
|
|
localHitNormal = tmp / dist;
|
|
region = 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
bCollide = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (dist > minDist)
|
|
{
|
|
minDist = dist;
|
|
closestPnt = pntReturn;
|
|
localHitNormal = planeEqn;
|
|
region = 3;
|
|
}
|
|
}
|
|
}
|
|
static int numChecks = 0;
|
|
numChecks++;
|
|
|
|
if (bCollide && minDist > -10000)
|
|
{
|
|
float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
|
|
float4 pOnB1 = tr(closestPnt);
|
|
//printf("dist ,%f,",minDist);
|
|
float actualDepth = minDist - radius;
|
|
if (actualDepth < 0)
|
|
{
|
|
//printf("actualDepth = ,%f,", actualDepth);
|
|
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
|
|
//printf("region=,%d,\n", region);
|
|
pOnB1[3] = actualDepth;
|
|
|
|
int dstIdx;
|
|
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
|
|
|
if (nGlobalContactsOut < maxContactCapacity)
|
|
{
|
|
dstIdx = nGlobalContactsOut;
|
|
nGlobalContactsOut++;
|
|
|
|
b3Contact4* c = &globalContactsOut[dstIdx];
|
|
c->m_worldNormalOnB = normalOnSurfaceB1;
|
|
c->setFrictionCoeff(0.7);
|
|
c->setRestituitionCoeff(0.f);
|
|
|
|
c->m_batchIdx = pairIndex;
|
|
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
|
|
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
|
|
c->m_worldPosB[0] = pOnB1;
|
|
int numPoints = 1;
|
|
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
|
|
} //if (dstIdx < numPairs)
|
|
}
|
|
} //if (hasCollision)
|
|
}
|
|
|
|
int computeContactConvexConvex2(
|
|
int pairIndex,
|
|
int bodyIndexA, int bodyIndexB,
|
|
int collidableIndexA, int collidableIndexB,
|
|
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
|
const b3AlignedObjectArray<b3Collidable>& collidables,
|
|
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
|
const b3AlignedObjectArray<b3Vector3>& convexVertices,
|
|
const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
|
|
const b3AlignedObjectArray<int>& convexIndices,
|
|
const b3AlignedObjectArray<b3GpuFace>& faces,
|
|
b3AlignedObjectArray<b3Contact4>& globalContactsOut,
|
|
int& nGlobalContactsOut,
|
|
int maxContactCapacity,
|
|
const b3AlignedObjectArray<b3Contact4>& oldContacts)
|
|
{
|
|
int contactIndex = -1;
|
|
b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
|
|
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
|
b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
|
|
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
|
|
|
b3ConvexPolyhedronData hullA, hullB;
|
|
|
|
b3Vector3 sepNormalWorldSpace;
|
|
|
|
b3Collidable colA = collidables[collidableIndexA];
|
|
hullA = convexShapes[colA.m_shapeIndex];
|
|
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
|
|
|
b3Collidable colB = collidables[collidableIndexB];
|
|
hullB = convexShapes[colB.m_shapeIndex];
|
|
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
|
|
|
// int contactCapacity = MAX_VERTS;
|
|
//int numContactsOut=0;
|
|
|
|
#ifdef _WIN32
|
|
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
|
|
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
|
|
#endif
|
|
|
|
bool foundSepAxis = findSeparatingAxis(hullA, hullB,
|
|
posA,
|
|
ornA,
|
|
posB,
|
|
ornB,
|
|
|
|
convexVertices, uniqueEdges, faces, convexIndices,
|
|
convexVertices, uniqueEdges, faces, convexIndices,
|
|
|
|
sepNormalWorldSpace);
|
|
|
|
if (foundSepAxis)
|
|
{
|
|
contactIndex = clipHullHullSingle(
|
|
bodyIndexA, bodyIndexB,
|
|
posA, ornA,
|
|
posB, ornB,
|
|
collidableIndexA, collidableIndexB,
|
|
&rigidBodies,
|
|
&globalContactsOut,
|
|
nGlobalContactsOut,
|
|
|
|
convexShapes,
|
|
convexShapes,
|
|
|
|
convexVertices,
|
|
uniqueEdges,
|
|
faces,
|
|
convexIndices,
|
|
|
|
convexVertices,
|
|
uniqueEdges,
|
|
faces,
|
|
convexIndices,
|
|
|
|
collidables,
|
|
collidables,
|
|
sepNormalWorldSpace,
|
|
maxContactCapacity);
|
|
}
|
|
|
|
return contactIndex;
|
|
}
|
|
|
|
void GpuSatCollision::computeConvexConvexContactsGPUSAT(b3OpenCLArray<b3Int4>* pairs, int nPairs,
|
|
const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
|
|
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
|
|
const b3OpenCLArray<b3Contact4>* oldContacts,
|
|
int maxContactCapacity,
|
|
int compoundPairCapacity,
|
|
const b3OpenCLArray<b3ConvexPolyhedronData>& convexData,
|
|
const b3OpenCLArray<b3Vector3>& gpuVertices,
|
|
const b3OpenCLArray<b3Vector3>& gpuUniqueEdges,
|
|
const b3OpenCLArray<b3GpuFace>& gpuFaces,
|
|
const b3OpenCLArray<int>& gpuIndices,
|
|
const b3OpenCLArray<b3Collidable>& gpuCollidables,
|
|
const b3OpenCLArray<b3GpuChildShape>& gpuChildShapes,
|
|
|
|
const b3OpenCLArray<b3Aabb>& clAabbsWorldSpace,
|
|
const b3OpenCLArray<b3Aabb>& clAabbsLocalSpace,
|
|
|
|
b3OpenCLArray<b3Vector3>& worldVertsB1GPU,
|
|
b3OpenCLArray<b3Int4>& clippingFacesOutGPU,
|
|
b3OpenCLArray<b3Vector3>& worldNormalsAGPU,
|
|
b3OpenCLArray<b3Vector3>& worldVertsA1GPU,
|
|
b3OpenCLArray<b3Vector3>& worldVertsB2GPU,
|
|
b3AlignedObjectArray<class b3OptimizedBvh*>& bvhDataUnused,
|
|
b3OpenCLArray<b3QuantizedBvhNode>* treeNodesGPU,
|
|
b3OpenCLArray<b3BvhSubtreeInfo>* subTreesGPU,
|
|
b3OpenCLArray<b3BvhInfo>* bvhInfo,
|
|
|
|
int numObjects,
|
|
int maxTriConvexPairCapacity,
|
|
b3OpenCLArray<b3Int4>& triangleConvexPairsOut,
|
|
int& numTriConvexPairsOut)
|
|
{
|
|
myframecount++;
|
|
|
|
if (!nPairs)
|
|
return;
|
|
|
|
#ifdef CHECK_ON_HOST
|
|
|
|
b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
|
|
treeNodesGPU->copyToHost(treeNodesCPU);
|
|
|
|
b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
|
|
subTreesGPU->copyToHost(subTreesCPU);
|
|
|
|
b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
|
|
bvhInfo->copyToHost(bvhInfoCPU);
|
|
|
|
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
|
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
|
|
|
b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
|
|
clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);
|
|
|
|
b3AlignedObjectArray<b3Int4> hostPairs;
|
|
pairs->copyToHost(hostPairs);
|
|
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
|
|
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
|
convexData.copyToHost(hostConvexData);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostVertices;
|
|
gpuVertices.copyToHost(hostVertices);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
|
|
gpuUniqueEdges.copyToHost(hostUniqueEdges);
|
|
b3AlignedObjectArray<b3GpuFace> hostFaces;
|
|
gpuFaces.copyToHost(hostFaces);
|
|
b3AlignedObjectArray<int> hostIndices;
|
|
gpuIndices.copyToHost(hostIndices);
|
|
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
|
gpuCollidables.copyToHost(hostCollidables);
|
|
|
|
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
|
gpuChildShapes.copyToHost(cpuChildShapes);
|
|
|
|
b3AlignedObjectArray<b3Int4> hostTriangleConvexPairs;
|
|
|
|
b3AlignedObjectArray<b3Contact4> hostContacts;
|
|
if (nContacts)
|
|
{
|
|
contactOut->copyToHost(hostContacts);
|
|
}
|
|
|
|
b3AlignedObjectArray<b3Contact4> oldHostContacts;
|
|
|
|
if (oldContacts->size())
|
|
{
|
|
oldContacts->copyToHost(oldHostContacts);
|
|
}
|
|
|
|
hostContacts.resize(maxContactCapacity);
|
|
|
|
for (int i = 0; i < nPairs; i++)
|
|
{
|
|
int bodyIndexA = hostPairs[i].x;
|
|
int bodyIndexB = hostPairs[i].y;
|
|
int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
|
|
int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
|
{
|
|
computeContactSphereConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
|
|
{
|
|
computeContactSphereConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
|
|
//printf("convex-sphere\n");
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
|
{
|
|
computeContactPlaneConvex(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
|
|
// printf("convex-plane\n");
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
|
{
|
|
computeContactPlaneConvex(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
|
|
// printf("plane-convex\n");
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
computeContactCompoundCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], hostAabbsWorldSpace, hostAabbsLocalSpace, hostVertices, hostUniqueEdges, hostIndices, hostFaces, &hostContacts[0],
|
|
nContacts, maxContactCapacity, treeNodesCPU, subTreesCPU, bvhInfoCPU);
|
|
// printf("convex-plane\n");
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
|
{
|
|
computeContactPlaneCompound(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
|
|
// printf("convex-plane\n");
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
|
{
|
|
computeContactPlaneCompound(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, &hostBodyBuf[0],
|
|
&hostCollidables[0], &hostConvexData[0], &cpuChildShapes[0], &hostVertices[0], &hostIndices[0], &hostFaces[0], &hostContacts[0], nContacts, maxContactCapacity);
|
|
// printf("plane-convex\n");
|
|
}
|
|
|
|
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
|
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
|
{
|
|
//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
|
|
int contactIndex = computeContactConvexConvex2(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, hostBodyBuf, hostCollidables, hostConvexData, hostVertices, hostUniqueEdges, hostIndices, hostFaces, hostContacts, nContacts, maxContactCapacity, oldHostContacts);
|
|
//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
|
|
|
if (contactIndex >= 0)
|
|
{
|
|
// printf("convex convex contactIndex = %d\n",contactIndex);
|
|
hostPairs[i].z = contactIndex;
|
|
}
|
|
// printf("plane-convex\n");
|
|
}
|
|
}
|
|
|
|
if (hostPairs.size())
|
|
{
|
|
pairs->copyFromHost(hostPairs);
|
|
}
|
|
|
|
hostContacts.resize(nContacts);
|
|
if (nContacts)
|
|
{
|
|
contactOut->copyFromHost(hostContacts);
|
|
}
|
|
else
|
|
{
|
|
contactOut->resize(0);
|
|
}
|
|
|
|
m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
|
|
//printf("(HOST) nContacts = %d\n",nContacts);
|
|
|
|
#else
|
|
|
|
{
|
|
if (nPairs)
|
|
{
|
|
m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
|
|
|
|
B3_PROFILE("primitiveContactsKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_primitiveContactsKernel, "m_primitiveContactsKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(nPairs);
|
|
launcher.setConst(maxContactCapacity);
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
|
|
nContacts = m_totalContactsOut.at(0);
|
|
contactOut->resize(nContacts);
|
|
}
|
|
}
|
|
|
|
#endif //CHECK_ON_HOST
|
|
|
|
B3_PROFILE("computeConvexConvexContactsGPUSAT");
|
|
// printf("nContacts = %d\n",nContacts);
|
|
|
|
m_sepNormals.resize(nPairs);
|
|
m_hasSeparatingNormals.resize(nPairs);
|
|
|
|
int concaveCapacity = maxTriConvexPairCapacity;
|
|
m_concaveSepNormals.resize(concaveCapacity);
|
|
m_concaveHasSeparatingNormals.resize(concaveCapacity);
|
|
m_numConcavePairsOut.resize(0);
|
|
m_numConcavePairsOut.push_back(0);
|
|
|
|
m_gpuCompoundPairs.resize(compoundPairCapacity);
|
|
|
|
m_gpuCompoundSepNormals.resize(compoundPairCapacity);
|
|
|
|
m_gpuHasCompoundSepNormals.resize(compoundPairCapacity);
|
|
|
|
m_numCompoundPairsOut.resize(0);
|
|
m_numCompoundPairsOut.push_back(0);
|
|
|
|
int numCompoundPairs = 0;
|
|
|
|
int numConcavePairs = 0;
|
|
|
|
{
|
|
clFinish(m_queue);
|
|
if (findSeparatingAxisOnGpu)
|
|
{
|
|
m_dmins.resize(nPairs);
|
|
if (splitSearchSepAxisConvex)
|
|
{
|
|
if (useMprGpu)
|
|
{
|
|
nContacts = m_totalContactsOut.at(0);
|
|
{
|
|
B3_PROFILE("mprPenetrationKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_mprPenetrationKernel, "mprPenetrationKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
|
|
launcher.setConst(maxContactCapacity);
|
|
launcher.setConst(nPairs);
|
|
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
/*
|
|
b3AlignedObjectArray<int>hostHasSepAxis;
|
|
m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
|
|
b3AlignedObjectArray<b3Vector3>hostSepAxis;
|
|
m_sepNormals.copyToHost(hostSepAxis);
|
|
*/
|
|
nContacts = m_totalContactsOut.at(0);
|
|
contactOut->resize(nContacts);
|
|
// printf("nContacts (after mprPenetrationKernel) = %d\n",nContacts);
|
|
if (nContacts > maxContactCapacity)
|
|
{
|
|
b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
|
nContacts = maxContactCapacity;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (1)
|
|
{
|
|
if (1)
|
|
{
|
|
{
|
|
B3_PROFILE("findSeparatingAxisVertexFaceKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_dmins.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel, "findSeparatingAxisVertexFaceKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(nPairs);
|
|
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
|
|
int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
|
|
|
|
{
|
|
B3_PROFILE("findSeparatingAxisEdgeEdgeKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_dmins.getBufferCL()),
|
|
b3BufferInfoCL(m_unitSphereDirections.getBufferCL(), true)
|
|
|
|
};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel, "findSeparatingAxisEdgeEdgeKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(numDirections);
|
|
launcher.setConst(nPairs);
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
}
|
|
if (useMprGpu)
|
|
{
|
|
B3_PROFILE("findSeparatingAxisUnitSphereKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(m_unitSphereDirections.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_dmins.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findSeparatingAxisUnitSphereKernel, "findSeparatingAxisUnitSphereKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
|
|
launcher.setConst(numDirections);
|
|
|
|
launcher.setConst(nPairs);
|
|
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
B3_PROFILE("findSeparatingAxisKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findSeparatingAxisKernel, "m_findSeparatingAxisKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(nPairs);
|
|
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
B3_PROFILE("findSeparatingAxisKernel CPU");
|
|
|
|
b3AlignedObjectArray<b3Int4> hostPairs;
|
|
pairs->copyToHost(hostPairs);
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
|
|
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
|
gpuCollidables.copyToHost(hostCollidables);
|
|
|
|
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
|
gpuChildShapes.copyToHost(cpuChildShapes);
|
|
|
|
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexShapeData;
|
|
convexData.copyToHost(hostConvexShapeData);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostVertices;
|
|
gpuVertices.copyToHost(hostVertices);
|
|
|
|
b3AlignedObjectArray<int> hostHasSepAxis;
|
|
hostHasSepAxis.resize(nPairs);
|
|
b3AlignedObjectArray<b3Vector3> hostSepAxis;
|
|
hostSepAxis.resize(nPairs);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
|
|
gpuUniqueEdges.copyToHost(hostUniqueEdges);
|
|
b3AlignedObjectArray<b3GpuFace> hostFaces;
|
|
gpuFaces.copyToHost(hostFaces);
|
|
|
|
b3AlignedObjectArray<int> hostIndices;
|
|
gpuIndices.copyToHost(hostIndices);
|
|
|
|
b3AlignedObjectArray<b3Contact4> hostContacts;
|
|
if (nContacts)
|
|
{
|
|
contactOut->copyToHost(hostContacts);
|
|
}
|
|
hostContacts.resize(maxContactCapacity);
|
|
int nGlobalContactsOut = nContacts;
|
|
|
|
for (int i = 0; i < nPairs; i++)
|
|
{
|
|
int bodyIndexA = hostPairs[i].x;
|
|
int bodyIndexB = hostPairs[i].y;
|
|
int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
|
|
int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
|
|
|
|
int shapeIndexA = hostCollidables[collidableIndexA].m_shapeIndex;
|
|
int shapeIndexB = hostCollidables[collidableIndexB].m_shapeIndex;
|
|
|
|
hostHasSepAxis[i] = 0;
|
|
|
|
//once the broadphase avoids static-static pairs, we can remove this test
|
|
if ((hostBodyBuf[bodyIndexA].m_invMass == 0) && (hostBodyBuf[bodyIndexB].m_invMass == 0))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ((hostCollidables[collidableIndexA].m_shapeType != SHAPE_CONVEX_HULL) || (hostCollidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
float dmin = FLT_MAX;
|
|
|
|
b3ConvexPolyhedronData* convexShapeA = &hostConvexShapeData[shapeIndexA];
|
|
b3ConvexPolyhedronData* convexShapeB = &hostConvexShapeData[shapeIndexB];
|
|
b3Vector3 posA = hostBodyBuf[bodyIndexA].m_pos;
|
|
b3Vector3 posB = hostBodyBuf[bodyIndexB].m_pos;
|
|
b3Quaternion ornA = hostBodyBuf[bodyIndexA].m_quat;
|
|
b3Quaternion ornB = hostBodyBuf[bodyIndexB].m_quat;
|
|
|
|
if (useGjk)
|
|
{
|
|
//first approximate the separating axis, to 'fail-proof' GJK+EPA or MPR
|
|
{
|
|
b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
|
|
b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
|
|
b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
|
|
b3Vector3 c1 = b3TransformPoint(c1local, posB, ornB);
|
|
b3Vector3 DeltaC2 = c0 - c1;
|
|
|
|
b3Vector3 sepAxis;
|
|
|
|
bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&sepAxis, &dmin);
|
|
|
|
if (hasSepAxisA)
|
|
{
|
|
bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&sepAxis, &dmin);
|
|
if (hasSepAxisB)
|
|
{
|
|
bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&sepAxis, &dmin, false);
|
|
|
|
if (hasEdgeEdge)
|
|
{
|
|
hostHasSepAxis[i] = 1;
|
|
hostSepAxis[i] = sepAxis;
|
|
hostSepAxis[i].w = dmin;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (hostHasSepAxis[i])
|
|
{
|
|
int pairIndex = i;
|
|
|
|
bool useMpr = true;
|
|
if (useMpr)
|
|
{
|
|
int res = 0;
|
|
float depth = 0.f;
|
|
b3Vector3 sepAxis2 = b3MakeVector3(1, 0, 0);
|
|
b3Vector3 resultPointOnBWorld = b3MakeVector3(0, 0, 0);
|
|
|
|
float depthOut;
|
|
b3Vector3 dirOut;
|
|
b3Vector3 posOut;
|
|
|
|
//res = b3MprPenetration(bodyIndexA,bodyIndexB,hostBodyBuf,hostConvexShapeData,hostCollidables,hostVertices,&mprConfig,&depthOut,&dirOut,&posOut);
|
|
res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB, &hostBodyBuf[0], &hostConvexShapeData[0], &hostCollidables[0], &hostVertices[0], &hostSepAxis[0], &hostHasSepAxis[0], &depthOut, &dirOut, &posOut);
|
|
depth = depthOut;
|
|
sepAxis2 = b3MakeVector3(-dirOut.x, -dirOut.y, -dirOut.z);
|
|
resultPointOnBWorld = posOut;
|
|
//hostHasSepAxis[i] = 0;
|
|
|
|
if (res == 0)
|
|
{
|
|
//add point?
|
|
//printf("depth = %f\n",depth);
|
|
//printf("normal = %f,%f,%f\n",dir.v[0],dir.v[1],dir.v[2]);
|
|
//qprintf("pos = %f,%f,%f\n",pos.v[0],pos.v[1],pos.v[2]);
|
|
|
|
float dist = 0.f;
|
|
|
|
const b3ConvexPolyhedronData& hullA = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexA].m_collidableIdx].m_shapeIndex];
|
|
const b3ConvexPolyhedronData& hullB = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexB].m_collidableIdx].m_shapeIndex];
|
|
|
|
if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist))
|
|
{
|
|
if (depth > dist)
|
|
{
|
|
float diff = depth - dist;
|
|
|
|
static float maxdiff = 0.f;
|
|
if (maxdiff < diff)
|
|
{
|
|
maxdiff = diff;
|
|
printf("maxdiff = %20.10f\n", maxdiff);
|
|
}
|
|
}
|
|
}
|
|
if (depth > dmin)
|
|
{
|
|
b3Vector3 oldAxis = hostSepAxis[i];
|
|
depth = dmin;
|
|
sepAxis2 = oldAxis;
|
|
}
|
|
|
|
if (b3TestSepAxis(&hullA, &hullB, posA, ornA, posB, ornB, &sepAxis2, &hostVertices[0], &hostVertices[0], &dist))
|
|
{
|
|
if (depth > dist)
|
|
{
|
|
float diff = depth - dist;
|
|
//printf("?diff = %f\n",diff );
|
|
static float maxdiff = 0.f;
|
|
if (maxdiff < diff)
|
|
{
|
|
maxdiff = diff;
|
|
printf("maxdiff = %20.10f\n", maxdiff);
|
|
}
|
|
}
|
|
//this is used for SAT
|
|
//hostHasSepAxis[i] = 1;
|
|
//hostSepAxis[i] = sepAxis2;
|
|
|
|
//add contact point
|
|
|
|
//int contactIndex = nGlobalContactsOut;
|
|
b3Contact4& newContact = hostContacts.at(nGlobalContactsOut);
|
|
nGlobalContactsOut++;
|
|
newContact.m_batchIdx = 0; //i;
|
|
newContact.m_bodyAPtrAndSignBit = (hostBodyBuf.at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
|
|
newContact.m_bodyBPtrAndSignBit = (hostBodyBuf.at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
|
|
|
|
newContact.m_frictionCoeffCmp = 45874;
|
|
newContact.m_restituitionCoeffCmp = 0;
|
|
|
|
static float maxDepth = 0.f;
|
|
|
|
if (depth > maxDepth)
|
|
{
|
|
maxDepth = depth;
|
|
printf("MPR maxdepth = %f\n", maxDepth);
|
|
}
|
|
|
|
resultPointOnBWorld.w = -depth;
|
|
newContact.m_worldPosB[0] = resultPointOnBWorld;
|
|
//b3Vector3 resultPointOnAWorld = resultPointOnBWorld+depth*sepAxis2;
|
|
newContact.m_worldNormalOnB = sepAxis2;
|
|
newContact.m_worldNormalOnB.w = (b3Scalar)1;
|
|
}
|
|
else
|
|
{
|
|
printf("rejected\n");
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//int contactIndex = computeContactConvexConvex2( i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
|
b3AlignedObjectArray<b3Contact4> oldHostContacts;
|
|
int result;
|
|
result = computeContactConvexConvex2( //hostPairs,
|
|
pairIndex,
|
|
bodyIndexA, bodyIndexB,
|
|
collidableIndexA, collidableIndexB,
|
|
hostBodyBuf,
|
|
hostCollidables,
|
|
hostConvexShapeData,
|
|
hostVertices,
|
|
hostUniqueEdges,
|
|
hostIndices,
|
|
hostFaces,
|
|
hostContacts,
|
|
nGlobalContactsOut,
|
|
maxContactCapacity,
|
|
oldHostContacts
|
|
//hostHasSepAxis,
|
|
//hostSepAxis
|
|
|
|
);
|
|
} //mpr
|
|
} //hostHasSepAxis[i] = 1;
|
|
}
|
|
else
|
|
{
|
|
b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
|
|
b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
|
|
b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
|
|
b3Vector3 c1 = b3TransformPoint(c1local, posB, ornB);
|
|
b3Vector3 DeltaC2 = c0 - c1;
|
|
|
|
b3Vector3 sepAxis;
|
|
|
|
bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&sepAxis, &dmin);
|
|
|
|
if (hasSepAxisA)
|
|
{
|
|
bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&sepAxis, &dmin);
|
|
if (hasSepAxisB)
|
|
{
|
|
bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
|
|
&sepAxis, &dmin, true);
|
|
|
|
if (hasEdgeEdge)
|
|
{
|
|
hostHasSepAxis[i] = 1;
|
|
hostSepAxis[i] = sepAxis;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (useGjkContacts) //nGlobalContactsOut>0)
|
|
{
|
|
//printf("nGlobalContactsOut=%d\n",nGlobalContactsOut);
|
|
nContacts = nGlobalContactsOut;
|
|
contactOut->copyFromHost(hostContacts);
|
|
|
|
m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
|
|
}
|
|
|
|
m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
|
|
m_sepNormals.copyFromHost(hostSepAxis);
|
|
|
|
/*
|
|
//double-check results from GPU (comment-out the 'else' so both paths are executed
|
|
b3AlignedObjectArray<int> checkHasSepAxis;
|
|
m_hasSeparatingNormals.copyToHost(checkHasSepAxis);
|
|
static int frameCount = 0;
|
|
frameCount++;
|
|
for (int i=0;i<nPairs;i++)
|
|
{
|
|
if (hostHasSepAxis[i] != checkHasSepAxis[i])
|
|
{
|
|
printf("at frameCount %d hostHasSepAxis[%d] = %d but checkHasSepAxis[i] = %d\n",
|
|
frameCount,i,hostHasSepAxis[i],checkHasSepAxis[i]);
|
|
}
|
|
}
|
|
//m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
|
|
// m_sepNormals.copyFromHost(hostSepAxis);
|
|
*/
|
|
}
|
|
|
|
numCompoundPairs = m_numCompoundPairsOut.at(0);
|
|
bool useGpuFindCompoundPairs = true;
|
|
if (useGpuFindCompoundPairs)
|
|
{
|
|
B3_PROFILE("findCompoundPairsKernel");
|
|
b3BufferInfoCL bInfo[] =
|
|
{
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsLocalSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL()),
|
|
b3BufferInfoCL(m_numCompoundPairsOut.getBufferCL()),
|
|
b3BufferInfoCL(subTreesGPU->getBufferCL()),
|
|
b3BufferInfoCL(treeNodesGPU->getBufferCL()),
|
|
b3BufferInfoCL(bvhInfo->getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findCompoundPairsKernel, "m_findCompoundPairsKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(nPairs);
|
|
launcher.setConst(compoundPairCapacity);
|
|
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
|
|
numCompoundPairs = m_numCompoundPairsOut.at(0);
|
|
//printf("numCompoundPairs =%d\n",numCompoundPairs );
|
|
if (numCompoundPairs)
|
|
{
|
|
//printf("numCompoundPairs=%d\n",numCompoundPairs);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
|
|
treeNodesGPU->copyToHost(treeNodesCPU);
|
|
|
|
b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
|
|
subTreesGPU->copyToHost(subTreesCPU);
|
|
|
|
b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
|
|
bvhInfo->copyToHost(bvhInfoCPU);
|
|
|
|
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
|
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
|
|
|
b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
|
|
clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);
|
|
|
|
b3AlignedObjectArray<b3Int4> hostPairs;
|
|
pairs->copyToHost(hostPairs);
|
|
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
|
|
b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
|
|
cpuCompoundPairsOut.resize(compoundPairCapacity);
|
|
|
|
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
|
gpuCollidables.copyToHost(hostCollidables);
|
|
|
|
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
|
gpuChildShapes.copyToHost(cpuChildShapes);
|
|
|
|
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
|
convexData.copyToHost(hostConvexData);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostVertices;
|
|
gpuVertices.copyToHost(hostVertices);
|
|
|
|
for (int pairIndex = 0; pairIndex < nPairs; pairIndex++)
|
|
{
|
|
int bodyIndexA = hostPairs[pairIndex].x;
|
|
int bodyIndexB = hostPairs[pairIndex].y;
|
|
int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
|
|
int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
|
|
if (cpuChildShapes.size())
|
|
{
|
|
findCompoundPairsKernel(
|
|
pairIndex,
|
|
bodyIndexA,
|
|
bodyIndexB,
|
|
collidableIndexA,
|
|
collidableIndexB,
|
|
&hostBodyBuf[0],
|
|
&hostCollidables[0],
|
|
&hostConvexData[0],
|
|
hostVertices,
|
|
hostAabbsWorldSpace,
|
|
hostAabbsLocalSpace,
|
|
&cpuChildShapes[0],
|
|
&cpuCompoundPairsOut[0],
|
|
&numCompoundPairs,
|
|
compoundPairCapacity,
|
|
treeNodesCPU,
|
|
subTreesCPU,
|
|
bvhInfoCPU);
|
|
}
|
|
}
|
|
|
|
m_numCompoundPairsOut.copyFromHostPointer(&numCompoundPairs, 1, 0, true);
|
|
if (numCompoundPairs)
|
|
{
|
|
b3CompoundOverlappingPair* ptr = (b3CompoundOverlappingPair*)&cpuCompoundPairsOut[0];
|
|
m_gpuCompoundPairs.copyFromHostPointer(ptr, numCompoundPairs, 0, true);
|
|
}
|
|
//cpuCompoundPairsOut
|
|
}
|
|
if (numCompoundPairs)
|
|
{
|
|
printf("numCompoundPairs=%d\n", numCompoundPairs);
|
|
}
|
|
|
|
if (numCompoundPairs > compoundPairCapacity)
|
|
{
|
|
b3Error("Exceeded compound pair capacity (%d/%d)\n", numCompoundPairs, compoundPairCapacity);
|
|
numCompoundPairs = compoundPairCapacity;
|
|
}
|
|
|
|
m_gpuCompoundPairs.resize(numCompoundPairs);
|
|
m_gpuHasCompoundSepNormals.resize(numCompoundPairs);
|
|
m_gpuCompoundSepNormals.resize(numCompoundPairs);
|
|
|
|
if (numCompoundPairs)
|
|
{
|
|
B3_PROFILE("processCompoundPairsPrimitivesKernel");
|
|
b3BufferInfoCL bInfo[] =
|
|
{
|
|
b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_processCompoundPairsPrimitivesKernel, "m_processCompoundPairsPrimitivesKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(numCompoundPairs);
|
|
launcher.setConst(maxContactCapacity);
|
|
|
|
int num = numCompoundPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
nContacts = m_totalContactsOut.at(0);
|
|
//printf("nContacts (after processCompoundPairsPrimitivesKernel) = %d\n",nContacts);
|
|
if (nContacts > maxContactCapacity)
|
|
{
|
|
b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
|
nContacts = maxContactCapacity;
|
|
}
|
|
}
|
|
|
|
if (numCompoundPairs)
|
|
{
|
|
B3_PROFILE("processCompoundPairsKernel");
|
|
b3BufferInfoCL bInfo[] =
|
|
{
|
|
b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(m_gpuCompoundSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_gpuHasCompoundSepNormals.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_processCompoundPairsKernel, "m_processCompoundPairsKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(numCompoundPairs);
|
|
|
|
int num = numCompoundPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
|
|
//printf("numConcave = %d\n",numConcave);
|
|
|
|
// printf("hostNormals.size()=%d\n",hostNormals.size());
|
|
//int numPairs = pairCount.at(0);
|
|
}
|
|
int vertexFaceCapacity = 64;
|
|
|
|
{
|
|
//now perform the tree query on GPU
|
|
|
|
if (treeNodesGPU->size() && treeNodesGPU->size())
|
|
{
|
|
if (bvhTraversalKernelGPU)
|
|
{
|
|
B3_PROFILE("m_bvhTraversalKernel");
|
|
|
|
numConcavePairs = m_numConcavePairsOut.at(0);
|
|
|
|
b3LauncherCL launcher(m_queue, m_bvhTraversalKernel, "m_bvhTraversalKernel");
|
|
launcher.setBuffer(pairs->getBufferCL());
|
|
launcher.setBuffer(bodyBuf->getBufferCL());
|
|
launcher.setBuffer(gpuCollidables.getBufferCL());
|
|
launcher.setBuffer(clAabbsWorldSpace.getBufferCL());
|
|
launcher.setBuffer(triangleConvexPairsOut.getBufferCL());
|
|
launcher.setBuffer(m_numConcavePairsOut.getBufferCL());
|
|
launcher.setBuffer(subTreesGPU->getBufferCL());
|
|
launcher.setBuffer(treeNodesGPU->getBufferCL());
|
|
launcher.setBuffer(bvhInfo->getBufferCL());
|
|
|
|
launcher.setConst(nPairs);
|
|
launcher.setConst(maxTriConvexPairCapacity);
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
numConcavePairs = m_numConcavePairsOut.at(0);
|
|
}
|
|
else
|
|
{
|
|
b3AlignedObjectArray<b3Int4> hostPairs;
|
|
pairs->copyToHost(hostPairs);
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
|
gpuCollidables.copyToHost(hostCollidables);
|
|
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
|
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
|
|
|
//int maxTriConvexPairCapacity,
|
|
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
|
triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
|
|
|
|
//int numTriConvexPairsOutHost=0;
|
|
numConcavePairs = 0;
|
|
//m_numConcavePairsOut
|
|
|
|
b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
|
|
treeNodesGPU->copyToHost(treeNodesCPU);
|
|
b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
|
|
subTreesGPU->copyToHost(subTreesCPU);
|
|
b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
|
|
bvhInfo->copyToHost(bvhInfoCPU);
|
|
//compute it...
|
|
|
|
volatile int hostNumConcavePairsOut = 0;
|
|
|
|
//
|
|
for (int i = 0; i < nPairs; i++)
|
|
{
|
|
b3BvhTraversal(&hostPairs.at(0),
|
|
&hostBodyBuf.at(0),
|
|
&hostCollidables.at(0),
|
|
&hostAabbsWorldSpace.at(0),
|
|
&triangleConvexPairsOutHost.at(0),
|
|
&hostNumConcavePairsOut,
|
|
&subTreesCPU.at(0),
|
|
&treeNodesCPU.at(0),
|
|
&bvhInfoCPU.at(0),
|
|
nPairs,
|
|
maxTriConvexPairCapacity,
|
|
i);
|
|
}
|
|
numConcavePairs = hostNumConcavePairsOut;
|
|
|
|
if (hostNumConcavePairsOut)
|
|
{
|
|
triangleConvexPairsOutHost.resize(hostNumConcavePairsOut);
|
|
triangleConvexPairsOut.copyFromHost(triangleConvexPairsOutHost);
|
|
}
|
|
//
|
|
|
|
m_numConcavePairsOut.resize(0);
|
|
m_numConcavePairsOut.push_back(numConcavePairs);
|
|
}
|
|
|
|
//printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);
|
|
|
|
if (numConcavePairs > maxTriConvexPairCapacity)
|
|
{
|
|
static int exceeded_maxTriConvexPairCapacity_count = 0;
|
|
b3Error("Exceeded the maxTriConvexPairCapacity (found %d but max is %d, it happened %d times)\n",
|
|
numConcavePairs, maxTriConvexPairCapacity, exceeded_maxTriConvexPairCapacity_count++);
|
|
numConcavePairs = maxTriConvexPairCapacity;
|
|
}
|
|
triangleConvexPairsOut.resize(numConcavePairs);
|
|
|
|
if (numConcavePairs)
|
|
{
|
|
clippingFacesOutGPU.resize(numConcavePairs);
|
|
worldNormalsAGPU.resize(numConcavePairs);
|
|
worldVertsA1GPU.resize(vertexFaceCapacity * (numConcavePairs));
|
|
worldVertsB1GPU.resize(vertexFaceCapacity * (numConcavePairs));
|
|
|
|
if (findConcaveSeparatingAxisKernelGPU)
|
|
{
|
|
/*
|
|
m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
|
|
clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
|
|
worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
|
|
worldNormalsAGPU.copyFromHost(worldNormalsACPU);
|
|
worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
|
|
*/
|
|
|
|
//now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
|
|
if (splitSearchSepAxisConcave)
|
|
{
|
|
//printf("numConcavePairs = %d\n",numConcavePairs);
|
|
m_dmins.resize(numConcavePairs);
|
|
{
|
|
B3_PROFILE("findConcaveSeparatingAxisVertexFaceKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
|
|
b3BufferInfoCL(m_dmins.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisVertexFaceKernel, "m_findConcaveSeparatingAxisVertexFaceKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
launcher.setConst(numConcavePairs);
|
|
|
|
int num = numConcavePairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
// numConcavePairs = 0;
|
|
if (1)
|
|
{
|
|
B3_PROFILE("findConcaveSeparatingAxisEdgeEdgeKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
|
|
b3BufferInfoCL(m_dmins.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisEdgeEdgeKernel, "m_findConcaveSeparatingAxisEdgeEdgeKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
launcher.setConst(numConcavePairs);
|
|
|
|
int num = numConcavePairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
|
|
// numConcavePairs = 0;
|
|
}
|
|
else
|
|
{
|
|
B3_PROFILE("findConcaveSeparatingAxisKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB1GPU.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel, "m_findConcaveSeparatingAxisKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
launcher.setConst(numConcavePairs);
|
|
|
|
int num = numConcavePairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
|
|
b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
|
|
b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
|
|
b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
|
|
|
|
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
|
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
|
|
//triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
|
gpuCollidables.copyToHost(hostCollidables);
|
|
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
|
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
|
|
|
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
|
convexData.copyToHost(hostConvexData);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostVertices;
|
|
gpuVertices.copyToHost(hostVertices);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
|
|
gpuUniqueEdges.copyToHost(hostUniqueEdges);
|
|
b3AlignedObjectArray<b3GpuFace> hostFaces;
|
|
gpuFaces.copyToHost(hostFaces);
|
|
b3AlignedObjectArray<int> hostIndices;
|
|
gpuIndices.copyToHost(hostIndices);
|
|
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
|
gpuChildShapes.copyToHost(cpuChildShapes);
|
|
|
|
b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
|
|
m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
|
|
concaveHasSeparatingNormalsCPU.resize(concaveSepNormalsHost.size());
|
|
|
|
b3GpuChildShape* childShapePointerCPU = 0;
|
|
if (cpuChildShapes.size())
|
|
childShapePointerCPU = &cpuChildShapes.at(0);
|
|
|
|
clippingFacesOutCPU.resize(clippingFacesOutGPU.size());
|
|
worldVertsA1CPU.resize(worldVertsA1GPU.size());
|
|
worldNormalsACPU.resize(worldNormalsAGPU.size());
|
|
worldVertsB1CPU.resize(worldVertsB1GPU.size());
|
|
|
|
for (int i = 0; i < numConcavePairs; i++)
|
|
{
|
|
b3FindConcaveSeparatingAxisKernel(&triangleConvexPairsOutHost.at(0),
|
|
&hostBodyBuf.at(0),
|
|
&hostCollidables.at(0),
|
|
&hostConvexData.at(0), &hostVertices.at(0), &hostUniqueEdges.at(0),
|
|
&hostFaces.at(0), &hostIndices.at(0), childShapePointerCPU,
|
|
&hostAabbsWorldSpace.at(0),
|
|
&concaveSepNormalsHost.at(0),
|
|
&clippingFacesOutCPU.at(0),
|
|
&worldVertsA1CPU.at(0),
|
|
&worldNormalsACPU.at(0),
|
|
&worldVertsB1CPU.at(0),
|
|
&concaveHasSeparatingNormalsCPU.at(0),
|
|
vertexFaceCapacity,
|
|
numConcavePairs, i);
|
|
};
|
|
|
|
m_concaveSepNormals.copyFromHost(concaveSepNormalsHost);
|
|
m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
|
|
clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
|
|
worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
|
|
worldNormalsAGPU.copyFromHost(worldNormalsACPU);
|
|
worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
|
|
}
|
|
// b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
|
|
// m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
|
|
// b3AlignedObjectArray<b3Int4> cpuConcavePairs;
|
|
// triangleConvexPairsOut.copyToHost(cpuConcavePairs);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (numConcavePairs)
|
|
{
|
|
if (numConcavePairs)
|
|
{
|
|
B3_PROFILE("findConcaveSphereContactsKernel");
|
|
nContacts = m_totalContactsOut.at(0);
|
|
// printf("nContacts1 = %d\n",nContacts);
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(triangleConvexPairsOut.getBufferCL()),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(clAabbsWorldSpace.getBufferCL(), true),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findConcaveSphereContactsKernel, "m_findConcaveSphereContactsKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
|
|
launcher.setConst(numConcavePairs);
|
|
launcher.setConst(maxContactCapacity);
|
|
|
|
int num = numConcavePairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
nContacts = m_totalContactsOut.at(0);
|
|
//printf("nContacts (after findConcaveSphereContactsKernel) = %d\n",nContacts);
|
|
|
|
//printf("nContacts2 = %d\n",nContacts);
|
|
|
|
if (nContacts >= maxContactCapacity)
|
|
{
|
|
b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
|
nContacts = maxContactCapacity;
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef __APPLE__
|
|
bool contactClippingOnGpu = true;
|
|
#else
|
|
bool contactClippingOnGpu = true;
|
|
#endif
|
|
|
|
if (contactClippingOnGpu)
|
|
{
|
|
m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
|
|
// printf("nContacts3 = %d\n",nContacts);
|
|
|
|
//B3_PROFILE("clipHullHullKernel");
|
|
|
|
bool breakupConcaveConvexKernel = true;
|
|
|
|
#ifdef __APPLE__
|
|
//actually, some Apple OpenCL platform/device combinations work fine...
|
|
breakupConcaveConvexKernel = true;
|
|
#endif
|
|
//concave-convex contact clipping
|
|
if (numConcavePairs)
|
|
{
|
|
// printf("numConcavePairs = %d\n", numConcavePairs);
|
|
// nContacts = m_totalContactsOut.at(0);
|
|
// printf("nContacts before = %d\n", nContacts);
|
|
|
|
if (breakupConcaveConvexKernel)
|
|
{
|
|
worldVertsB2GPU.resize(vertexFaceCapacity * numConcavePairs);
|
|
|
|
//clipFacesAndFindContacts
|
|
|
|
if (clipConcaveFacesAndFindContactsCPU)
|
|
{
|
|
b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
|
|
b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
|
|
b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
|
|
|
|
clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
|
|
worldVertsA1GPU.copyToHost(worldVertsA1CPU);
|
|
worldNormalsAGPU.copyToHost(worldNormalsACPU);
|
|
worldVertsB1GPU.copyToHost(worldVertsB1CPU);
|
|
|
|
b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
|
|
m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU);
|
|
|
|
b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
|
|
m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
|
|
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
|
|
worldVertsB2CPU.resize(worldVertsB2GPU.size());
|
|
|
|
for (int i = 0; i < numConcavePairs; i++)
|
|
{
|
|
clipFacesAndFindContactsKernel(&concaveSepNormalsHost.at(0),
|
|
&concaveHasSeparatingNormalsCPU.at(0),
|
|
&clippingFacesOutCPU.at(0),
|
|
&worldVertsA1CPU.at(0),
|
|
&worldNormalsACPU.at(0),
|
|
&worldVertsB1CPU.at(0),
|
|
&worldVertsB2CPU.at(0),
|
|
vertexFaceCapacity,
|
|
i);
|
|
}
|
|
|
|
clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
|
|
worldVertsB2GPU.copyFromHost(worldVertsB2CPU);
|
|
}
|
|
else
|
|
{
|
|
if (1)
|
|
{
|
|
B3_PROFILE("clipFacesAndFindContacts");
|
|
//nContacts = m_totalContactsOut.at(0);
|
|
//int h = m_hasSeparatingNormals.at(0);
|
|
//int4 p = clippingFacesOutGPU.at(0);
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB2GPU.getBufferCL())};
|
|
b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
|
|
launcher.setConst(numConcavePairs);
|
|
int debugMode = 0;
|
|
launcher.setConst(debugMode);
|
|
int num = numConcavePairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
//int bla = m_totalContactsOut.at(0);
|
|
}
|
|
}
|
|
//contactReduction
|
|
{
|
|
int newContactCapacity = nContacts + numConcavePairs;
|
|
contactOut->reserve(newContactCapacity);
|
|
if (reduceConcaveContactsOnGPU)
|
|
{
|
|
// printf("newReservation = %d\n",newReservation);
|
|
{
|
|
B3_PROFILE("newContactReductionKernel");
|
|
b3BufferInfoCL bInfo[] =
|
|
{
|
|
b3BufferInfoCL(triangleConvexPairsOut.getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_concaveHasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB2GPU.getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_newContactReductionKernel, "m_newContactReductionKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
launcher.setConst(newContactCapacity);
|
|
launcher.setConst(numConcavePairs);
|
|
int num = numConcavePairs;
|
|
|
|
launcher.launch1D(num);
|
|
}
|
|
nContacts = m_totalContactsOut.at(0);
|
|
contactOut->resize(nContacts);
|
|
|
|
//printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
|
|
}
|
|
else
|
|
{
|
|
volatile int nGlobalContactsOut = nContacts;
|
|
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
|
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
|
|
b3AlignedObjectArray<int> concaveHasSeparatingNormalsCPU;
|
|
m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU);
|
|
|
|
b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
|
|
m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
|
|
|
|
b3AlignedObjectArray<b3Contact4> hostContacts;
|
|
if (nContacts)
|
|
{
|
|
contactOut->copyToHost(hostContacts);
|
|
}
|
|
hostContacts.resize(newContactCapacity);
|
|
|
|
b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
|
|
|
|
clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
|
|
worldVertsB2GPU.copyToHost(worldVertsB2CPU);
|
|
|
|
for (int i = 0; i < numConcavePairs; i++)
|
|
{
|
|
b3NewContactReductionKernel(&triangleConvexPairsOutHost.at(0),
|
|
&hostBodyBuf.at(0),
|
|
&concaveSepNormalsHost.at(0),
|
|
&concaveHasSeparatingNormalsCPU.at(0),
|
|
&hostContacts.at(0),
|
|
&clippingFacesOutCPU.at(0),
|
|
&worldVertsB2CPU.at(0),
|
|
&nGlobalContactsOut,
|
|
vertexFaceCapacity,
|
|
newContactCapacity,
|
|
numConcavePairs,
|
|
i);
|
|
}
|
|
|
|
nContacts = nGlobalContactsOut;
|
|
m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
|
|
// nContacts = m_totalContactsOut.at(0);
|
|
//contactOut->resize(nContacts);
|
|
hostContacts.resize(nContacts);
|
|
//printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
|
|
contactOut->copyFromHost(hostContacts);
|
|
}
|
|
}
|
|
//re-use?
|
|
}
|
|
else
|
|
{
|
|
B3_PROFILE("clipHullHullConcaveConvexKernel");
|
|
nContacts = m_totalContactsOut.at(0);
|
|
int newContactCapacity = contactOut->capacity();
|
|
|
|
//printf("contactOut5 = %d\n",nContacts);
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(triangleConvexPairsOut.getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(m_concaveSepNormals.getBufferCL()),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel, "m_clipHullHullConcaveConvexKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(newContactCapacity);
|
|
launcher.setConst(numConcavePairs);
|
|
int num = numConcavePairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
nContacts = m_totalContactsOut.at(0);
|
|
contactOut->resize(nContacts);
|
|
//printf("contactOut6 = %d\n",nContacts);
|
|
b3AlignedObjectArray<b3Contact4> cpuContacts;
|
|
contactOut->copyToHost(cpuContacts);
|
|
}
|
|
// printf("nContacts after = %d\n", nContacts);
|
|
} //numConcavePairs
|
|
|
|
//convex-convex contact clipping
|
|
|
|
bool breakupKernel = false;
|
|
|
|
#ifdef __APPLE__
|
|
breakupKernel = true;
|
|
#endif
|
|
|
|
#ifdef CHECK_ON_HOST
|
|
bool computeConvexConvex = false;
|
|
#else
|
|
bool computeConvexConvex = true;
|
|
#endif //CHECK_ON_HOST
|
|
if (computeConvexConvex)
|
|
{
|
|
B3_PROFILE("clipHullHullKernel");
|
|
if (breakupKernel)
|
|
{
|
|
worldVertsB1GPU.resize(vertexFaceCapacity * nPairs);
|
|
clippingFacesOutGPU.resize(nPairs);
|
|
worldNormalsAGPU.resize(nPairs);
|
|
worldVertsA1GPU.resize(vertexFaceCapacity * nPairs);
|
|
worldVertsB2GPU.resize(vertexFaceCapacity * nPairs);
|
|
|
|
if (findConvexClippingFacesGPU)
|
|
{
|
|
B3_PROFILE("findClippingFacesKernel");
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB1GPU.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_findClippingFacesKernel, "m_findClippingFacesKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
launcher.setConst(nPairs);
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
else
|
|
{
|
|
float minDist = -1e30f;
|
|
float maxDist = 0.02f;
|
|
|
|
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
|
|
convexData.copyToHost(hostConvexData);
|
|
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
|
gpuCollidables.copyToHost(hostCollidables);
|
|
|
|
b3AlignedObjectArray<int> hostHasSepNormals;
|
|
m_hasSeparatingNormals.copyToHost(hostHasSepNormals);
|
|
b3AlignedObjectArray<b3Vector3> cpuSepNormals;
|
|
m_sepNormals.copyToHost(cpuSepNormals);
|
|
|
|
b3AlignedObjectArray<b3Int4> hostPairs;
|
|
pairs->copyToHost(hostPairs);
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
|
|
//worldVertsB1GPU.resize(vertexFaceCapacity*nPairs);
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
|
|
worldVertsB1GPU.copyToHost(worldVertsB1CPU);
|
|
|
|
b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
|
|
clippingFacesOutGPU.copyToHost(clippingFacesOutCPU);
|
|
|
|
b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
|
|
worldNormalsACPU.resize(nPairs);
|
|
|
|
b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
|
|
worldVertsA1CPU.resize(worldVertsA1GPU.size());
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostVertices;
|
|
gpuVertices.copyToHost(hostVertices);
|
|
b3AlignedObjectArray<b3GpuFace> hostFaces;
|
|
gpuFaces.copyToHost(hostFaces);
|
|
b3AlignedObjectArray<int> hostIndices;
|
|
gpuIndices.copyToHost(hostIndices);
|
|
|
|
for (int i = 0; i < nPairs; i++)
|
|
{
|
|
int bodyIndexA = hostPairs[i].x;
|
|
int bodyIndexB = hostPairs[i].y;
|
|
|
|
int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
|
|
int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
|
|
|
|
int shapeIndexA = hostCollidables[collidableIndexA].m_shapeIndex;
|
|
int shapeIndexB = hostCollidables[collidableIndexB].m_shapeIndex;
|
|
|
|
if (hostHasSepNormals[i])
|
|
{
|
|
b3FindClippingFaces(cpuSepNormals[i],
|
|
&hostConvexData[shapeIndexA],
|
|
&hostConvexData[shapeIndexB],
|
|
hostBodyBuf[bodyIndexA].m_pos, hostBodyBuf[bodyIndexA].m_quat,
|
|
hostBodyBuf[bodyIndexB].m_pos, hostBodyBuf[bodyIndexB].m_quat,
|
|
&worldVertsA1CPU.at(0), &worldNormalsACPU.at(0),
|
|
&worldVertsB1CPU.at(0),
|
|
vertexFaceCapacity, minDist, maxDist,
|
|
&hostVertices.at(0), &hostFaces.at(0),
|
|
&hostIndices.at(0),
|
|
&hostVertices.at(0), &hostFaces.at(0),
|
|
&hostIndices.at(0), &clippingFacesOutCPU.at(0), i);
|
|
}
|
|
}
|
|
|
|
clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
|
|
worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
|
|
worldNormalsAGPU.copyFromHost(worldNormalsACPU);
|
|
worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
|
|
}
|
|
|
|
///clip face B against face A, reduce contacts and append them to a global contact array
|
|
if (1)
|
|
{
|
|
if (clipConvexFacesAndFindContactsCPU)
|
|
{
|
|
//b3AlignedObjectArray<b3Int4> hostPairs;
|
|
//pairs->copyToHost(hostPairs);
|
|
|
|
b3AlignedObjectArray<b3Vector3> hostSepNormals;
|
|
m_sepNormals.copyToHost(hostSepNormals);
|
|
b3AlignedObjectArray<int> hostHasSepAxis;
|
|
m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
|
|
|
|
b3AlignedObjectArray<b3Int4> hostClippingFaces;
|
|
clippingFacesOutGPU.copyToHost(hostClippingFaces);
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
|
|
worldVertsB2CPU.resize(vertexFaceCapacity * nPairs);
|
|
|
|
b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
|
|
worldVertsA1GPU.copyToHost(worldVertsA1CPU);
|
|
b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
|
|
worldNormalsAGPU.copyToHost(worldNormalsACPU);
|
|
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
|
|
worldVertsB1GPU.copyToHost(worldVertsB1CPU);
|
|
|
|
/*
|
|
__global const b3Float4* separatingNormals,
|
|
__global const int* hasSeparatingAxis,
|
|
__global b3Int4* clippingFacesOut,
|
|
__global b3Float4* worldVertsA1,
|
|
__global b3Float4* worldNormalsA1,
|
|
__global b3Float4* worldVertsB1,
|
|
__global b3Float4* worldVertsB2,
|
|
int vertexFaceCapacity,
|
|
int pairIndex
|
|
*/
|
|
for (int i = 0; i < nPairs; i++)
|
|
{
|
|
clipFacesAndFindContactsKernel(
|
|
&hostSepNormals.at(0),
|
|
&hostHasSepAxis.at(0),
|
|
&hostClippingFaces.at(0),
|
|
&worldVertsA1CPU.at(0),
|
|
&worldNormalsACPU.at(0),
|
|
&worldVertsB1CPU.at(0),
|
|
&worldVertsB2CPU.at(0),
|
|
|
|
vertexFaceCapacity,
|
|
i);
|
|
}
|
|
|
|
clippingFacesOutGPU.copyFromHost(hostClippingFaces);
|
|
worldVertsB2GPU.copyFromHost(worldVertsB2CPU);
|
|
}
|
|
else
|
|
{
|
|
B3_PROFILE("clipFacesAndFindContacts");
|
|
//nContacts = m_totalContactsOut.at(0);
|
|
//int h = m_hasSeparatingNormals.at(0);
|
|
//int4 p = clippingFacesOutGPU.at(0);
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsA1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB1GPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB2GPU.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts, "m_clipFacesAndFindContacts");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
|
|
launcher.setConst(nPairs);
|
|
int debugMode = 0;
|
|
launcher.setConst(debugMode);
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
}
|
|
|
|
{
|
|
nContacts = m_totalContactsOut.at(0);
|
|
//printf("nContacts = %d\n",nContacts);
|
|
|
|
int newContactCapacity = nContacts + nPairs;
|
|
contactOut->reserve(newContactCapacity);
|
|
|
|
if (reduceConvexContactsOnGPU)
|
|
{
|
|
{
|
|
B3_PROFILE("newContactReductionKernel");
|
|
b3BufferInfoCL bInfo[] =
|
|
{
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(clippingFacesOutGPU.getBufferCL()),
|
|
b3BufferInfoCL(worldVertsB2GPU.getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
|
|
b3LauncherCL launcher(m_queue, m_newContactReductionKernel, "m_newContactReductionKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(vertexFaceCapacity);
|
|
launcher.setConst(newContactCapacity);
|
|
launcher.setConst(nPairs);
|
|
int num = nPairs;
|
|
|
|
launcher.launch1D(num);
|
|
}
|
|
nContacts = m_totalContactsOut.at(0);
|
|
contactOut->resize(nContacts);
|
|
}
|
|
else
|
|
{
|
|
volatile int nGlobalContactsOut = nContacts;
|
|
b3AlignedObjectArray<b3Int4> hostPairs;
|
|
pairs->copyToHost(hostPairs);
|
|
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
|
|
bodyBuf->copyToHost(hostBodyBuf);
|
|
b3AlignedObjectArray<b3Vector3> hostSepNormals;
|
|
m_sepNormals.copyToHost(hostSepNormals);
|
|
b3AlignedObjectArray<int> hostHasSepAxis;
|
|
m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
|
|
b3AlignedObjectArray<b3Contact4> hostContactsOut;
|
|
contactOut->copyToHost(hostContactsOut);
|
|
hostContactsOut.resize(newContactCapacity);
|
|
|
|
b3AlignedObjectArray<b3Int4> hostClippingFaces;
|
|
clippingFacesOutGPU.copyToHost(hostClippingFaces);
|
|
b3AlignedObjectArray<b3Vector3> worldVertsB2CPU;
|
|
worldVertsB2GPU.copyToHost(worldVertsB2CPU);
|
|
|
|
for (int i = 0; i < nPairs; i++)
|
|
{
|
|
b3NewContactReductionKernel(&hostPairs.at(0),
|
|
&hostBodyBuf.at(0),
|
|
&hostSepNormals.at(0),
|
|
&hostHasSepAxis.at(0),
|
|
&hostContactsOut.at(0),
|
|
&hostClippingFaces.at(0),
|
|
&worldVertsB2CPU.at(0),
|
|
&nGlobalContactsOut,
|
|
vertexFaceCapacity,
|
|
newContactCapacity,
|
|
nPairs,
|
|
i);
|
|
}
|
|
|
|
nContacts = nGlobalContactsOut;
|
|
m_totalContactsOut.copyFromHostPointer(&nContacts, 1, 0, true);
|
|
hostContactsOut.resize(nContacts);
|
|
//printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts);
|
|
contactOut->copyFromHost(hostContactsOut);
|
|
}
|
|
// b3Contact4 pt = contactOut->at(0);
|
|
// printf("nContacts = %d\n",nContacts);
|
|
}
|
|
}
|
|
}
|
|
else //breakupKernel
|
|
{
|
|
if (nPairs)
|
|
{
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(pairs->getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(m_sepNormals.getBufferCL()),
|
|
b3BufferInfoCL(m_hasSeparatingNormals.getBufferCL()),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
b3LauncherCL launcher(m_queue, m_clipHullHullKernel, "m_clipHullHullKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(nPairs);
|
|
launcher.setConst(maxContactCapacity);
|
|
|
|
int num = nPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
|
|
nContacts = m_totalContactsOut.at(0);
|
|
if (nContacts >= maxContactCapacity)
|
|
{
|
|
b3Error("Exceeded contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
|
nContacts = maxContactCapacity;
|
|
}
|
|
contactOut->resize(nContacts);
|
|
}
|
|
}
|
|
|
|
int nCompoundsPairs = m_gpuCompoundPairs.size();
|
|
|
|
if (nCompoundsPairs)
|
|
{
|
|
b3BufferInfoCL bInfo[] = {
|
|
b3BufferInfoCL(m_gpuCompoundPairs.getBufferCL(), true),
|
|
b3BufferInfoCL(bodyBuf->getBufferCL(), true),
|
|
b3BufferInfoCL(gpuCollidables.getBufferCL(), true),
|
|
b3BufferInfoCL(convexData.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuVertices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuUniqueEdges.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuFaces.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuIndices.getBufferCL(), true),
|
|
b3BufferInfoCL(gpuChildShapes.getBufferCL(), true),
|
|
b3BufferInfoCL(m_gpuCompoundSepNormals.getBufferCL(), true),
|
|
b3BufferInfoCL(m_gpuHasCompoundSepNormals.getBufferCL(), true),
|
|
b3BufferInfoCL(contactOut->getBufferCL()),
|
|
b3BufferInfoCL(m_totalContactsOut.getBufferCL())};
|
|
b3LauncherCL launcher(m_queue, m_clipCompoundsHullHullKernel, "m_clipCompoundsHullHullKernel");
|
|
launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
|
|
launcher.setConst(nCompoundsPairs);
|
|
launcher.setConst(maxContactCapacity);
|
|
|
|
int num = nCompoundsPairs;
|
|
launcher.launch1D(num);
|
|
clFinish(m_queue);
|
|
|
|
nContacts = m_totalContactsOut.at(0);
|
|
if (nContacts > maxContactCapacity)
|
|
{
|
|
b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
|
nContacts = maxContactCapacity;
|
|
}
|
|
contactOut->resize(nContacts);
|
|
} //if nCompoundsPairs
|
|
}
|
|
} //contactClippingOnGpu
|
|
|
|
//printf("nContacts end = %d\n",nContacts);
|
|
|
|
//printf("frameCount = %d\n",frameCount++);
|
|
}
|