Remove bullet3
huxingyi
parent
3f4cf7b149
commit
5da1a7f6f2
211
dust3d.pro
211
dust3d.pro
|
|
@ -449,9 +449,6 @@ HEADERS += src/mousepicker.h
|
|||
SOURCES += src/paintmode.cpp
|
||||
HEADERS += src/paintmode.h
|
||||
|
||||
SOURCES += src/ragdoll.cpp
|
||||
HEADERS += src/ragdoll.h
|
||||
|
||||
SOURCES += src/proceduralanimation.cpp
|
||||
HEADERS += src/proceduralanimation.h
|
||||
|
||||
|
|
@ -566,214 +563,6 @@ win32 {
|
|||
LIBS += -L$${SOURCE_ROOT}thirdparty/instant-meshes/build/ext_build/tbb/RelWithDebInfo -ltbb
|
||||
}
|
||||
|
||||
INCLUDEPATH += thirdparty/bullet3/src
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/LinearMath/btAlignedAllocator.cpp
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btAlignedAllocator.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/Dynamics/btRigidBody.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/Dynamics/btRigidBody.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionObject.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/LinearMath/btVector3.cpp
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btVector3.h
|
||||
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btScalar.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/LinearMath/btSerializer.cpp
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btSerializer.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/LinearMath/btQuickprof.cpp
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btQuickprof.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btCollisionShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btCollisionShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btCapsuleShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexInternalShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btManifoldResult.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btTriangleCallback.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDispatcher.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btUnionFind.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConcaveShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConcaveShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvt.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvt.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/LinearMath/btGeometryUtil.cpp
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btGeometryUtil.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/LinearMath/btConvexHullComputer.cpp
|
||||
HEADERS += thirdparty/bullet3/src/LinearMath/btConvexHullComputer.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btMiniSDF.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btMiniSDF.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btSphereShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btSphereShape.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
|
||||
|
||||
SOURCES += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btBoxShape.cpp
|
||||
HEADERS += thirdparty/bullet3/src/BulletCollision/CollisionShapes/btBoxShape.h
|
||||
|
||||
INCLUDEPATH += thirdparty/quickjs/quickjs-2019-07-09-dust3d
|
||||
|
||||
DEFINES += "CONFIG_VERSION=\"\\\"2019-07-09\\\"\""
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
#include "posemeshcreator.h"
|
||||
#include "poserconstruct.h"
|
||||
#include "posedocument.h"
|
||||
#include "ragdoll.h"
|
||||
#include "boundingboxmesh.h"
|
||||
|
||||
MotionsGenerator::MotionsGenerator(RigType rigType,
|
||||
|
|
@ -105,49 +104,6 @@ float MotionsGenerator::calculatePoseDuration(const QUuid &poseId)
|
|||
return totalDuration;
|
||||
}
|
||||
|
||||
const std::vector<std::pair<float, JointNodeTree>> &MotionsGenerator::getProceduralAnimation(ProceduralAnimation proceduralAnimation,
|
||||
const JointNodeTree *initialJointNodeTree)
|
||||
{
|
||||
auto findResult = m_proceduralAnimations.find((int)proceduralAnimation);
|
||||
if (findResult != m_proceduralAnimations.end())
|
||||
return findResult->second;
|
||||
std::vector<std::pair<float, JointNodeTree>> &resultFrames = m_proceduralAnimations[(int)proceduralAnimation];
|
||||
if (ProceduralAnimation::FallToDeath == proceduralAnimation) {
|
||||
#if ENABLE_PROCEDURAL_DEBUG
|
||||
std::vector<Model *> &resultPreviews = m_proceduralDebugPreviews[(int)proceduralAnimation];
|
||||
#endif
|
||||
RagDoll ragdoll(&m_rigBones, initialJointNodeTree);
|
||||
float stepSeconds = 1.0 / 60;
|
||||
float maxSeconds = 1.5;
|
||||
int maxSteps = maxSeconds / stepSeconds;
|
||||
int steps = 0;
|
||||
while (steps < maxSteps && ragdoll.stepSimulation(stepSeconds)) {
|
||||
resultFrames.push_back(std::make_pair(stepSeconds * 2, ragdoll.getStepJointNodeTree()));
|
||||
#if ENABLE_PROCEDURAL_DEBUG
|
||||
Model *preview = buildBoundingBoxMesh(ragdoll.getStepBonePositions());
|
||||
resultPreviews.push_back(preview);
|
||||
#endif
|
||||
++steps;
|
||||
}
|
||||
}
|
||||
if (resultFrames.empty()) {
|
||||
resultFrames.push_back(std::make_pair(0, JointNodeTree(&m_rigBones)));
|
||||
}
|
||||
return resultFrames;
|
||||
}
|
||||
|
||||
float MotionsGenerator::calculateProceduralAnimationDuration(ProceduralAnimation proceduralAnimation,
|
||||
const JointNodeTree *initialJointNodeTree)
|
||||
{
|
||||
const auto &result = getProceduralAnimation(proceduralAnimation,
|
||||
initialJointNodeTree);
|
||||
float totalDuration = 0;
|
||||
for (const auto &it: result) {
|
||||
totalDuration += it.first;
|
||||
}
|
||||
return totalDuration;
|
||||
}
|
||||
|
||||
float MotionsGenerator::calculateMotionDuration(const QUuid &motionId, std::set<QUuid> &visited)
|
||||
{
|
||||
const std::vector<MotionClip> *motionClips = findMotionClips(motionId);
|
||||
|
|
@ -165,14 +121,7 @@ float MotionsGenerator::calculateMotionDuration(const QUuid &motionId, std::set<
|
|||
totalDuration += clip.duration;
|
||||
else if (clip.clipType == MotionClipType::Pose)
|
||||
totalDuration += calculatePoseDuration(clip.linkToId);
|
||||
else if (clip.clipType == MotionClipType::ProceduralAnimation) {
|
||||
const JointNodeTree *previousJointNodeTree = nullptr;
|
||||
if (clipIndex > 1) {
|
||||
previousJointNodeTree = findClipEndJointNodeTree((*motionClips)[clipIndex - 2]);
|
||||
}
|
||||
totalDuration += calculateProceduralAnimationDuration(clip.proceduralAnimation,
|
||||
previousJointNodeTree);
|
||||
} else if (clip.clipType == MotionClipType::Motion)
|
||||
else if (clip.clipType == MotionClipType::Motion)
|
||||
totalDuration += calculateMotionDuration(clip.linkToId, visited);
|
||||
}
|
||||
return totalDuration;
|
||||
|
|
@ -200,13 +149,6 @@ void MotionsGenerator::generateMotion(const QUuid &motionId, std::set<QUuid> &vi
|
|||
clip.duration = calculateMotionDuration(clip.linkToId, subVisited);
|
||||
} else if (clip.clipType == MotionClipType::Pose) {
|
||||
clip.duration = calculatePoseDuration(clip.linkToId);
|
||||
} else if (clip.clipType == MotionClipType::ProceduralAnimation) {
|
||||
const JointNodeTree *previousJointNodeTree = nullptr;
|
||||
if (clipIndex > 1) {
|
||||
previousJointNodeTree = findClipEndJointNodeTree((*motionClips)[clipIndex - 2]);
|
||||
}
|
||||
clip.duration = calculateProceduralAnimationDuration(clip.proceduralAnimation,
|
||||
previousJointNodeTree);
|
||||
}
|
||||
timePoints.push_back(totalDuration);
|
||||
totalDuration += clip.duration;
|
||||
|
|
@ -288,27 +230,6 @@ void MotionsGenerator::generateMotion(const QUuid &motionId, std::set<QUuid> &vi
|
|||
generateMotion(progressClip.linkToId, visited, outcomes);
|
||||
progress += progressClip.duration;
|
||||
continue;
|
||||
} else if (MotionClipType::ProceduralAnimation == progressClip.clipType) {
|
||||
const JointNodeTree *beginJointNodeTree = nullptr;
|
||||
if (clipIndex > 0) {
|
||||
beginJointNodeTree = findClipEndJointNodeTree((*motionClips)[clipIndex - 1]);
|
||||
}
|
||||
const auto &frames = getProceduralAnimation(progressClip.proceduralAnimation,
|
||||
beginJointNodeTree);
|
||||
float clipDuration = std::max((float)0.0001, progressClip.duration);
|
||||
int frame = clipLocalProgress * frames.size() / clipDuration;
|
||||
if (frame >= (int)frames.size())
|
||||
frame = frames.size() - 1;
|
||||
if (frame >= 0 && frame < (int)frames.size()) {
|
||||
#if ENABLE_PROCEDURAL_DEBUG
|
||||
if (nullptr != previews)
|
||||
previews->push_back(m_proceduralDebugPreviews[(int)progressClip.proceduralAnimation][frame]);
|
||||
#endif
|
||||
outcomes.push_back({progress - lastProgress, frames[frame].second});
|
||||
lastProgress = progress;
|
||||
}
|
||||
progress += interval;
|
||||
continue;
|
||||
}
|
||||
progress += interval;
|
||||
}
|
||||
|
|
@ -354,11 +275,6 @@ const JointNodeTree *MotionsGenerator::findClipBeginJointNodeTree(const MotionCl
|
|||
return findClipBeginJointNodeTree((*motionClips)[0]);
|
||||
}
|
||||
return nullptr;
|
||||
} else if (MotionClipType::ProceduralAnimation == clip.clipType) {
|
||||
const auto &result = getProceduralAnimation(clip.proceduralAnimation);
|
||||
if (!result.empty())
|
||||
return &result[0].second;
|
||||
return nullptr;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
|
@ -377,11 +293,6 @@ const JointNodeTree *MotionsGenerator::findClipEndJointNodeTree(const MotionClip
|
|||
return findClipEndJointNodeTree((*motionClips)[motionClips->size() - 1]);
|
||||
}
|
||||
return nullptr;
|
||||
} else if (MotionClipType::ProceduralAnimation == clip.clipType) {
|
||||
const auto &result = getProceduralAnimation(clip.proceduralAnimation);
|
||||
if (!result.empty())
|
||||
return &result[result.size() - 1].second;
|
||||
return nullptr;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -47,10 +47,6 @@ private:
|
|||
void generatePreviewsForOutcomes(const std::vector<std::pair<float, JointNodeTree>> &outcomes, std::vector<std::pair<float, Model *>> &previews);
|
||||
float calculateMotionDuration(const QUuid &motionId, std::set<QUuid> &visited);
|
||||
float calculatePoseDuration(const QUuid &poseId);
|
||||
float calculateProceduralAnimationDuration(ProceduralAnimation proceduralAnimation,
|
||||
const JointNodeTree *initialJointNodeTree=nullptr);
|
||||
const std::vector<std::pair<float, JointNodeTree>> &getProceduralAnimation(ProceduralAnimation proceduralAnimation,
|
||||
const JointNodeTree *initialJointNodeTree=nullptr);
|
||||
|
||||
RigType m_rigType = RigType::None;
|
||||
std::vector<RiggerBone> m_rigBones;
|
||||
|
|
|
|||
383
src/ragdoll.cpp
383
src/ragdoll.cpp
|
|
@ -1,383 +0,0 @@
|
|||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#include <LinearMath/btDefaultMotionState.h>
|
||||
#include <LinearMath/btAlignedAllocator.h>
|
||||
#include <BulletCollision/CollisionShapes/btCapsuleShape.h>
|
||||
#include <BulletCollision/CollisionShapes/btBoxShape.h>
|
||||
#include <BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h>
|
||||
#include <BulletDynamics/ConstraintSolver/btFixedConstraint.h>
|
||||
#include <BulletDynamics/ConstraintSolver/btTypedConstraint.h>
|
||||
#include <QQuaternion>
|
||||
#include <QtMath>
|
||||
#include <QMatrix4x4>
|
||||
#include <iostream>
|
||||
#include "ragdoll.h"
|
||||
#include "poser.h"
|
||||
|
||||
RagDoll::RagDoll(const std::vector<RiggerBone> *rigBones,
|
||||
const JointNodeTree *initialJointNodeTree) :
|
||||
m_jointNodeTree(rigBones),
|
||||
m_stepJointNodeTree(nullptr == initialJointNodeTree ? rigBones : *initialJointNodeTree)
|
||||
{
|
||||
if (nullptr == rigBones || rigBones->empty())
|
||||
return;
|
||||
|
||||
if (m_stepJointNodeTree.nodes().size() != m_jointNodeTree.nodes().size())
|
||||
return;
|
||||
|
||||
m_bones = *rigBones;
|
||||
|
||||
if (nullptr != initialJointNodeTree) {
|
||||
m_jointNodeTree.calculateBonePositions(&m_boneInitialPositions,
|
||||
initialJointNodeTree,
|
||||
rigBones);
|
||||
} else {
|
||||
m_boneInitialPositions.resize(m_bones.size());
|
||||
for (size_t i = 0; i < m_bones.size(); ++i) {
|
||||
const auto &bone = m_bones[i];
|
||||
m_boneInitialPositions[i] = std::make_pair(bone.headPosition, bone.tailPosition);
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto &bone: m_bones) {
|
||||
const auto &bonePosition = m_boneInitialPositions[bone.index];
|
||||
auto radius = (bone.headRadius + bone.tailRadius) * 0.5;
|
||||
m_stepBonePositions.push_back(std::make_tuple(bonePosition.first,
|
||||
bonePosition.second, bone.headRadius, bone.tailRadius, bone.color));
|
||||
float groundY = bonePosition.first.y() - radius;
|
||||
if (groundY < m_groundY)
|
||||
m_groundY = groundY;
|
||||
groundY = bonePosition.second.y() - radius;
|
||||
if (groundY < m_groundY)
|
||||
m_groundY = groundY;
|
||||
}
|
||||
|
||||
createDynamicsWorld();
|
||||
|
||||
std::vector<QString> boneNames;
|
||||
for (const auto &bone: *rigBones) {
|
||||
m_boneNameToIndexMap[bone.name] = bone.index;
|
||||
boneNames.push_back(bone.name);
|
||||
}
|
||||
Poser::fetchChains(boneNames, m_chains);
|
||||
|
||||
for (const auto &bone: *rigBones) {
|
||||
const auto &headPosition = m_boneInitialPositions[bone.index].first;
|
||||
const auto &tailPosition = m_boneInitialPositions[bone.index].second;
|
||||
float radius = qMin(bone.headRadius, bone.tailRadius);
|
||||
float height = headPosition.distanceToPoint(tailPosition);
|
||||
QVector3D middlePosition = (headPosition + tailPosition) * 0.5;
|
||||
m_boneLengthMap[bone.name] = height;
|
||||
m_boneRadiusMap[bone.name] = radius;
|
||||
m_boneMiddleMap[bone.name] = middlePosition;
|
||||
}
|
||||
|
||||
std::set<std::pair<QString, QString>> constraintPairs;
|
||||
|
||||
for (const auto &bone: m_bones) {
|
||||
if (0 == bone.index)
|
||||
continue;
|
||||
|
||||
const auto &headPosition = m_boneInitialPositions[bone.index].first;
|
||||
const auto &tailPosition = m_boneInitialPositions[bone.index].second;
|
||||
float height = m_boneLengthMap[bone.name];
|
||||
float radius = m_boneRadiusMap[bone.name];
|
||||
float mass = radius * height;
|
||||
|
||||
btCollisionShape *shape = nullptr;
|
||||
|
||||
if (bone.name.startsWith("Spine")) {
|
||||
float halfHeight = height * 0.5f;
|
||||
float revisedRadius = radius < halfHeight ? radius : halfHeight;
|
||||
mass *= 5.0f;
|
||||
shape = new btBoxShape(btVector3(revisedRadius, halfHeight, revisedRadius * 0.1f));
|
||||
} else {
|
||||
shape = new btCapsuleShape(btScalar(radius), btScalar(height));
|
||||
}
|
||||
|
||||
shape->setUserIndex(bone.index);
|
||||
|
||||
m_boneShapes[bone.name] = shape;
|
||||
|
||||
btTransform transform;
|
||||
const auto &middlePosition = m_boneMiddleMap[bone.name];
|
||||
transform.setIdentity();
|
||||
transform.setOrigin(btVector3(
|
||||
btScalar(middlePosition.x()),
|
||||
btScalar(middlePosition.y()),
|
||||
btScalar(middlePosition.z())
|
||||
));
|
||||
QVector3D to = (tailPosition - headPosition).normalized();
|
||||
QVector3D from = QVector3D(0, 1, 0);
|
||||
QQuaternion rotation = QQuaternion::rotationTo(from, to);
|
||||
btQuaternion btRotation(btScalar(rotation.x()), btScalar(rotation.y()), btScalar(rotation.z()),
|
||||
btScalar(rotation.scalar()));
|
||||
transform.getBasis().setRotation(btRotation);
|
||||
|
||||
btRigidBody *body = createRigidBody(btScalar(mass), transform, shape);
|
||||
|
||||
//body->setDamping(btScalar(0.05), btScalar(0.85));
|
||||
//body->setDeactivationTime(btScalar(0.8));
|
||||
//body->setSleepingThresholds(btScalar(1.6), btScalar(2.5));
|
||||
|
||||
m_boneBodies[bone.name] = body;
|
||||
}
|
||||
|
||||
for (const auto &bone: m_bones) {
|
||||
if (0 == bone.index || -1 == bone.parent)
|
||||
continue;
|
||||
if (0 == bone.parent) {
|
||||
if ("Spine1" == bone.name)
|
||||
continue;
|
||||
auto findFirstSpine = m_boneNameToIndexMap.find("Spine1");
|
||||
if (findFirstSpine == m_boneNameToIndexMap.end())
|
||||
continue;
|
||||
addConstraint(bone, m_bones[findFirstSpine->second], true);
|
||||
continue;
|
||||
}
|
||||
addConstraint(bone, m_bones[bone.parent]);
|
||||
}
|
||||
|
||||
for (const auto &bone: m_bones) {
|
||||
for (const auto &childIndex: bone.children) {
|
||||
for (const auto &otherIndex: bone.children) {
|
||||
if (childIndex == otherIndex)
|
||||
continue;
|
||||
m_boneBodies[m_bones[childIndex].name]->setIgnoreCollisionCheck(m_boneBodies[m_bones[otherIndex].name], true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto &it: m_chains) {
|
||||
const auto &name = it.second[0];
|
||||
const auto &leftBody = m_boneBodies.find(name);
|
||||
if (leftBody == m_boneBodies.end())
|
||||
continue;
|
||||
const QString left = "Left";
|
||||
if (name.startsWith(left)) {
|
||||
QString pairedName = "Right" + name.mid(left.length());
|
||||
const auto &rightBody = m_boneBodies.find(pairedName);
|
||||
if (rightBody == m_boneBodies.end())
|
||||
continue;
|
||||
leftBody->second->setIgnoreCollisionCheck(rightBody->second, true);
|
||||
rightBody->second->setIgnoreCollisionCheck(leftBody->second, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RagDoll::addConstraint(const RiggerBone &child, const RiggerBone &parent, bool isBorrowedParent)
|
||||
{
|
||||
btRigidBody *parentBoneBody = m_boneBodies[parent.name];
|
||||
btRigidBody *childBoneBody = m_boneBodies[child.name];
|
||||
|
||||
if (nullptr == parentBoneBody || nullptr == childBoneBody)
|
||||
return;
|
||||
|
||||
bool reversed = isBorrowedParent;
|
||||
|
||||
std::cout << "addConstraint parent:" << parent.name.toUtf8().constData() << " child:" << child.name.toUtf8().constData() << " reversed:" << reversed << std::endl;
|
||||
|
||||
float parentLength = m_boneLengthMap[parent.name];
|
||||
float childLength = m_boneLengthMap[child.name];
|
||||
const btVector3 btPivotA(0, (reversed ? -1 : 1) * parentLength * 0.5, 0.0f);
|
||||
const btVector3 btPivotB(0, -childLength * 0.5, 0.0f);
|
||||
|
||||
btTransform localA;
|
||||
btTransform localB;
|
||||
|
||||
localA.setIdentity();
|
||||
localB.setIdentity();
|
||||
localA.setOrigin(btPivotA);
|
||||
localB.setOrigin(btPivotB);
|
||||
|
||||
if (child.name.startsWith("Spine") || child.name.startsWith("Virtual")) {
|
||||
btFixedConstraint *fixedConstraint = new btFixedConstraint(*parentBoneBody, *childBoneBody,
|
||||
localA, localB);
|
||||
m_world->addConstraint(fixedConstraint, true);
|
||||
m_boneConstraints.push_back(fixedConstraint);
|
||||
return;
|
||||
}
|
||||
|
||||
btGeneric6DofConstraint *g6dConstraint = nullptr;
|
||||
bool useLinearReferenceFrameA = true;
|
||||
g6dConstraint = new btGeneric6DofConstraint(*parentBoneBody, *childBoneBody, localA, localB, useLinearReferenceFrameA);
|
||||
if ("LeftLimb1_Joint1" == parent.name || "LeftLimb2_Joint1" == parent.name) {
|
||||
g6dConstraint->setAngularLowerLimit(btVector3(SIMD_EPSILON, -SIMD_EPSILON, -SIMD_EPSILON));
|
||||
g6dConstraint->setAngularUpperLimit(btVector3(-SIMD_PI * 0.7f, SIMD_EPSILON, SIMD_EPSILON));
|
||||
} else if ("RightLimb1_Joint1" == parent.name || "RightLimb2_Joint1" == parent.name) {
|
||||
g6dConstraint->setAngularLowerLimit(btVector3(SIMD_EPSILON, -SIMD_EPSILON, -SIMD_EPSILON));
|
||||
g6dConstraint->setAngularUpperLimit(btVector3(SIMD_PI * 0.7f, SIMD_EPSILON, SIMD_EPSILON));
|
||||
} else if ("LeftLimb1_Joint1" == child.name || "LeftLimb2_Joint1" == child.name) {
|
||||
g6dConstraint->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5, -SIMD_EPSILON, -SIMD_EPSILON));
|
||||
g6dConstraint->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.8, SIMD_EPSILON, SIMD_HALF_PI * 0.6f));
|
||||
} else if ("RightLimb1_Joint1" == child.name || "RightLimb2_Joint1" == child.name) {
|
||||
g6dConstraint->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5, -SIMD_EPSILON, -SIMD_HALF_PI * 0.6f));
|
||||
g6dConstraint->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.8, SIMD_EPSILON, SIMD_EPSILON));
|
||||
} else {
|
||||
g6dConstraint->setAngularLowerLimit(btVector3(-SIMD_EPSILON, -SIMD_EPSILON, -SIMD_EPSILON));
|
||||
g6dConstraint->setAngularUpperLimit(btVector3(SIMD_EPSILON, SIMD_EPSILON, SIMD_EPSILON));
|
||||
}
|
||||
|
||||
m_world->addConstraint(g6dConstraint, true);
|
||||
m_boneConstraints.push_back(g6dConstraint);
|
||||
}
|
||||
|
||||
RagDoll::~RagDoll()
|
||||
{
|
||||
for (auto &constraint: m_boneConstraints) {
|
||||
delete constraint;
|
||||
}
|
||||
m_boneConstraints.clear();
|
||||
for (auto &body: m_boneBodies) {
|
||||
if (nullptr == body.second)
|
||||
continue;
|
||||
m_world->removeRigidBody(body.second);
|
||||
delete body.second->getMotionState();
|
||||
delete body.second;
|
||||
}
|
||||
m_boneBodies.clear();
|
||||
for (auto &shape: m_boneShapes) {
|
||||
delete shape.second;
|
||||
}
|
||||
m_boneShapes.clear();
|
||||
|
||||
delete m_groundShape;
|
||||
delete m_groundBody;
|
||||
|
||||
delete m_world;
|
||||
|
||||
delete m_collisionConfiguration;
|
||||
delete m_collisionDispather;
|
||||
delete m_broadphase;
|
||||
delete m_constraintSolver;
|
||||
}
|
||||
|
||||
void RagDoll::createDynamicsWorld()
|
||||
{
|
||||
m_collisionConfiguration = new btDefaultCollisionConfiguration();
|
||||
m_collisionDispather = new btCollisionDispatcher(m_collisionConfiguration);
|
||||
m_broadphase = new btDbvtBroadphase();
|
||||
m_constraintSolver = new btSequentialImpulseConstraintSolver();
|
||||
|
||||
m_world = new btDiscreteDynamicsWorld(m_collisionDispather, m_broadphase, m_constraintSolver, m_collisionConfiguration);
|
||||
m_world->setGravity(btVector3(0, -10, 0));
|
||||
|
||||
m_world->getSolverInfo().m_solverMode |= SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
|
||||
m_world->getSolverInfo().m_numIterations = 5;
|
||||
|
||||
m_groundShape = new btBoxShape(btVector3(btScalar(250.), btScalar(250.), btScalar(250.)));
|
||||
|
||||
btTransform groundTransform;
|
||||
groundTransform.setIdentity();
|
||||
groundTransform.setOrigin(btVector3(0, m_groundY - 250, 0));
|
||||
m_groundBody = createRigidBody(0, groundTransform, m_groundShape);
|
||||
}
|
||||
|
||||
bool RagDoll::stepSimulation(float amount)
|
||||
{
|
||||
bool positionChanged = false;
|
||||
m_world->stepSimulation(btScalar(amount));
|
||||
|
||||
if (m_boneBodies.empty())
|
||||
return positionChanged;
|
||||
|
||||
m_stepJointNodeTree = m_jointNodeTree;
|
||||
|
||||
auto updateToNewPosition = [&](const QString &boneName, const btTransform &btWorldTransform, int jointIndex) {
|
||||
float halfHeight = m_boneLengthMap[boneName] * 0.5;
|
||||
btVector3 oldBoneHead(btScalar(0.0f), btScalar(-halfHeight), btScalar(0.0f));
|
||||
btVector3 oldBoneTail(btScalar(0.0f), btScalar(halfHeight), btScalar(0.0f));
|
||||
btVector3 newBoneHead = btWorldTransform * oldBoneHead;
|
||||
btVector3 newBoneTail = btWorldTransform * oldBoneTail;
|
||||
std::get<0>(m_stepBonePositions[jointIndex]) = QVector3D(newBoneHead.x(), newBoneHead.y(), newBoneHead.z());
|
||||
std::get<1>(m_stepBonePositions[jointIndex]) = QVector3D(newBoneTail.x(), newBoneTail.y(), newBoneTail.z());
|
||||
};
|
||||
for (const auto &it: m_boneShapes) {
|
||||
btTransform btWorldTransform;
|
||||
const auto *body = m_boneBodies[it.first];
|
||||
if (nullptr == body)
|
||||
continue;
|
||||
if (body->isActive()) {
|
||||
positionChanged = true;
|
||||
}
|
||||
if (body->getMotionState()) {
|
||||
body->getMotionState()->getWorldTransform(btWorldTransform);
|
||||
} else {
|
||||
btWorldTransform = body->getWorldTransform();
|
||||
}
|
||||
int jointIndex = it.second->getUserIndex();
|
||||
if (-1 == jointIndex)
|
||||
continue;
|
||||
updateToNewPosition(it.first, btWorldTransform, jointIndex);
|
||||
}
|
||||
|
||||
if (!m_bones[0].children.empty()) {
|
||||
QVector3D sumOfRootBoneTail;
|
||||
for (const auto &childIndex: m_bones[0].children) {
|
||||
sumOfRootBoneTail += std::get<0>(m_stepBonePositions[childIndex]);
|
||||
}
|
||||
QVector3D modelTranslation = sumOfRootBoneTail / m_bones[0].children.size();
|
||||
m_stepJointNodeTree.addTranslation(0,
|
||||
QVector3D(0, modelTranslation.y() - m_stepJointNodeTree.nodes()[0].translation.y(), 0));
|
||||
}
|
||||
|
||||
std::vector<QVector3D> directions(m_stepBonePositions.size());
|
||||
for (size_t index = 0; index < m_stepBonePositions.size(); ++index) {
|
||||
if (index >= m_bones.size())
|
||||
continue;
|
||||
const auto &boneNode = m_bones[index];
|
||||
directions[index] = boneNode.tailPosition - boneNode.headPosition;
|
||||
}
|
||||
std::function<void(size_t index, const QQuaternion &rotation)> rotateChildren;
|
||||
rotateChildren = [&](size_t index, const QQuaternion &rotation) {
|
||||
const auto &bone = m_bones[index];
|
||||
for (const auto &childIndex: bone.children) {
|
||||
directions[childIndex] = rotation.rotatedVector(directions[childIndex]);
|
||||
rotateChildren(childIndex, rotation);
|
||||
}
|
||||
};
|
||||
for (size_t index = 1; index < m_stepBonePositions.size(); ++index) {
|
||||
if (m_bones[index].name.startsWith("Virtual"))
|
||||
continue;
|
||||
QQuaternion rotation;
|
||||
const auto &oldDirection = directions[index];
|
||||
QVector3D newDirection = std::get<1>(m_stepBonePositions[index]) - std::get<0>(m_stepBonePositions[index]);
|
||||
rotation = QQuaternion::rotationTo(oldDirection.normalized(), newDirection.normalized());
|
||||
m_stepJointNodeTree.updateRotation(index, rotation);
|
||||
rotateChildren(index, rotation);
|
||||
}
|
||||
|
||||
m_stepJointNodeTree.recalculateTransformMatrices();
|
||||
return positionChanged;
|
||||
}
|
||||
|
||||
const JointNodeTree &RagDoll::getStepJointNodeTree()
|
||||
{
|
||||
return m_stepJointNodeTree;
|
||||
}
|
||||
|
||||
const std::vector<std::tuple<QVector3D, QVector3D, float, float, QColor>> &RagDoll::getStepBonePositions()
|
||||
{
|
||||
return m_stepBonePositions;
|
||||
}
|
||||
|
||||
btRigidBody *RagDoll::createRigidBody(btScalar mass, const btTransform &startTransform, btCollisionShape *shape)
|
||||
{
|
||||
bool isDynamic = !qFuzzyIsNull(mass);
|
||||
|
||||
btVector3 localInertia(0, 0, 0);
|
||||
if (isDynamic)
|
||||
shape->calculateLocalInertia(mass, localInertia);
|
||||
|
||||
btDefaultMotionState *myMotionState = new btDefaultMotionState(startTransform);
|
||||
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
|
||||
btRigidBody *body = new btRigidBody(rbInfo);
|
||||
body->setFriction(1.0f);
|
||||
body->setRollingFriction(1.0f);
|
||||
body->setSpinningFriction(1.0f);
|
||||
|
||||
m_world->addRigidBody(body);
|
||||
|
||||
return body;
|
||||
}
|
||||
|
|
@ -1,66 +0,0 @@
|
|||
#ifndef DUST3D_RAGDOLL_H
|
||||
#define DUST3D_RAGDOLL_H
|
||||
#include <QObject>
|
||||
#include <BulletDynamics/Dynamics/btDynamicsWorld.h>
|
||||
#include <BulletDynamics/Dynamics/btRigidBody.h>
|
||||
#include <BulletCollision/CollisionShapes/btCollisionShape.h>
|
||||
#include <BulletDynamics/ConstraintSolver/btTypedConstraint.h>
|
||||
#include <LinearMath/btVector3.h>
|
||||
#include <LinearMath/btScalar.h>
|
||||
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
|
||||
#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
|
||||
#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
|
||||
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
|
||||
#include <BulletCollision/CollisionShapes/btBoxShape.h>
|
||||
#include <QString>
|
||||
#include <map>
|
||||
#include <tuple>
|
||||
#include <QStringList>
|
||||
#include <QColor>
|
||||
#include "rigger.h"
|
||||
#include "jointnodetree.h"
|
||||
|
||||
class RagDoll : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
RagDoll(const std::vector<RiggerBone> *rigBones,
|
||||
const JointNodeTree *initialJointNodeTree=nullptr);
|
||||
~RagDoll();
|
||||
bool stepSimulation(float amount);
|
||||
const JointNodeTree &getStepJointNodeTree();
|
||||
const std::vector<std::tuple<QVector3D, QVector3D, float, float, QColor>> &getStepBonePositions();
|
||||
|
||||
private:
|
||||
btDefaultCollisionConfiguration *m_collisionConfiguration = nullptr;
|
||||
btCollisionDispatcher *m_collisionDispather = nullptr;
|
||||
btDbvtBroadphase *m_broadphase = nullptr;
|
||||
btSequentialImpulseConstraintSolver *m_constraintSolver = nullptr;
|
||||
btDynamicsWorld *m_world = nullptr;
|
||||
btCollisionShape *m_groundShape = nullptr;
|
||||
btRigidBody *m_groundBody = nullptr;
|
||||
float m_groundY = 0;
|
||||
std::vector<std::pair<QVector3D, QVector3D>> m_boneInitialPositions;
|
||||
|
||||
std::map<QString, btCollisionShape *> m_boneShapes;
|
||||
std::map<QString, btRigidBody *> m_boneBodies;
|
||||
std::vector<btTypedConstraint *> m_boneConstraints;
|
||||
|
||||
std::map<QString, QVector3D> m_boneMiddleMap;
|
||||
std::map<QString, float> m_boneLengthMap;
|
||||
std::map<QString, float> m_boneRadiusMap;
|
||||
|
||||
JointNodeTree m_jointNodeTree;
|
||||
JointNodeTree m_stepJointNodeTree;
|
||||
std::vector<RiggerBone> m_bones;
|
||||
std::vector<std::tuple<QVector3D, QVector3D, float, float, QColor>> m_stepBonePositions;
|
||||
|
||||
std::map<QString, int> m_boneNameToIndexMap;
|
||||
std::map<QString, std::vector<QString>> m_chains;
|
||||
|
||||
btRigidBody *createRigidBody(btScalar mass, const btTransform &startTransform, btCollisionShape *shape);
|
||||
void createDynamicsWorld();
|
||||
void addConstraint(const RiggerBone &child, const RiggerBone &parent, bool isBorrowedParent=false);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -1,45 +0,0 @@
|
|||
Bullet Physics is created by Erwin Coumans with contributions from the following authors / copyright holders:
|
||||
|
||||
AMD
|
||||
Apple
|
||||
Yunfei Bai
|
||||
Steve Baker
|
||||
Gino van den Bergen
|
||||
Jeff Bingham
|
||||
Nicola Candussi
|
||||
Erin Catto
|
||||
Lawrence Chai
|
||||
Erwin Coumans
|
||||
Disney Animation
|
||||
Benjamin Ellenberger
|
||||
Christer Ericson
|
||||
Google
|
||||
Dirk Gregorius
|
||||
Marcus Hennix
|
||||
Jasmine Hsu
|
||||
MBSim Development Team
|
||||
Takahiro Harada
|
||||
Simon Hobbs
|
||||
John Hsu
|
||||
Ole Kniemeyer
|
||||
Jay Lee
|
||||
Francisco Leon
|
||||
lunkhound
|
||||
Vsevolod Klementjev
|
||||
Phil Knight
|
||||
John McCutchan
|
||||
Steven Peters
|
||||
Roman Ponomarev
|
||||
Nathanael Presson
|
||||
Gabor PUHR
|
||||
Arthur Shek
|
||||
Russel Smith
|
||||
Sony
|
||||
Jakub Stephien
|
||||
Marten Svanfeldt
|
||||
Jie Tan
|
||||
Pierre Terdiman
|
||||
Steven Thompson
|
||||
Tamas Umenhoffer
|
||||
|
||||
If your name is missing, please send an email to erwin.coumans@gmail.com or file an issue at http://github.com/bulletphysics/bullet3
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
|
||||
The files in this repository are licensed under the zlib license, except for the files under 'Extras' and examples/ThirdPartyLibs.
|
||||
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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.
|
||||
|
|
@ -1,118 +0,0 @@
|
|||
[](https://travis-ci.org/bulletphysics/bullet3)
|
||||
[](https://ci.appveyor.com/project/erwincoumans/bullet3)
|
||||
|
||||
# Bullet Physics SDK
|
||||
|
||||
This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
|
||||
|
||||
New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
|
||||
|
||||
The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
|
||||
The steps towards a new API is in a nutshell:
|
||||
|
||||
1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
|
||||
2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
|
||||
3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
|
||||
4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
|
||||
5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
|
||||
|
||||
You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
|
||||
|
||||
## Requirements for Bullet 2
|
||||
|
||||
A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android,
|
||||
but should likely work on any platform with C++ compiler.
|
||||
Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too.
|
||||
|
||||
## Contributors and Coding Style information
|
||||
|
||||
https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
|
||||
|
||||
## Requirements for experimental OpenCL GPGPU support
|
||||
|
||||
The entire collision detection and rigid body dynamics can be executed on the GPU.
|
||||
|
||||
A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
|
||||
We succesfully tested the software under Windows, Linux and Mac OSX.
|
||||
The software currently doesn't work on OpenCL CPU devices. It might run
|
||||
on a laptop GPU but performance will not likely be very good. Note that
|
||||
often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
|
||||
track down the issue, but more work is required to cover all OpenCL kernels.
|
||||
|
||||
## License
|
||||
|
||||
All source code files are licensed under the permissive zlib license
|
||||
(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
|
||||
|
||||
## Build instructions for Bullet using premake. You can also use cmake instead.
|
||||
|
||||
**Windows**
|
||||
|
||||
Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
|
||||
When asked, convert the projects to a newer version of Visual Studio.
|
||||
If you installed Python in the C:\ root directory, the batch file should find it automatically.
|
||||
Otherwise, edit this batch file to choose where Python include/lib directories are located.
|
||||
|
||||
**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
|
||||
|
||||
Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
|
||||
You can connect from Python pybullet to the sandbox using:
|
||||
|
||||
```
|
||||
import pybullet as p
|
||||
p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86.10",1234)
|
||||
```
|
||||
|
||||
**Linux and Mac OSX gnu make**
|
||||
|
||||
Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)
|
||||
|
||||
In a terminal type:
|
||||
|
||||
./build_cmake_pybullet_double.sh
|
||||
|
||||
This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet.
|
||||
The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser.
|
||||
|
||||
You can also build Bullet using premake. There are premake executables in the build3 folder.
|
||||
Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
|
||||
Using premake:
|
||||
```
|
||||
cd build3
|
||||
./premake4_linux --double gmake
|
||||
./premake4_linux64 --double gmake
|
||||
./premake4_osx --double --enable_pybullet gmake
|
||||
```
|
||||
Then
|
||||
```
|
||||
cd gmake
|
||||
make
|
||||
```
|
||||
|
||||
Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries.
|
||||
|
||||
**Mac OSX Xcode**
|
||||
|
||||
Click on build3/xcode4.command or in a terminal window execute
|
||||
|
||||
./premake_osx xcode4
|
||||
|
||||
## Usage
|
||||
|
||||
The App_ExampleBrowser executables will be located in the bin folder.
|
||||
You can just run it though a terminal/command prompt, or by clicking it.
|
||||
|
||||
|
||||
```
|
||||
[--start_demo_name="Demo Name"] Start with a selected demo
|
||||
[--mp4=moviename.mp4] Create a mp4 movie of the window, requires ffmpeg installed
|
||||
[--mouse_move_multiplier=0.400000] Set the mouse move sensitivity
|
||||
[--mouse_wheel_multiplier=0.01] Set the mouse wheel sensitivity
|
||||
[--background_color_red= 0.9] Set the red component for background color. Same for green and blue
|
||||
[--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666)
|
||||
```
|
||||
|
||||
You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
|
||||
Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
|
||||
|
||||
Check out the docs folder and the Bullet physics forums for further information.
|
||||
|
|
@ -1 +0,0 @@
|
|||
2.88
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_BROADPHASE_CALLBACK_H
|
||||
#define B3_BROADPHASE_CALLBACK_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
struct b3BroadphaseProxy;
|
||||
|
||||
struct b3BroadphaseAabbCallback
|
||||
{
|
||||
virtual ~b3BroadphaseAabbCallback() {}
|
||||
virtual bool process(const b3BroadphaseProxy* proxy) = 0;
|
||||
};
|
||||
|
||||
struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
|
||||
{
|
||||
///added some cached data to accelerate ray-AABB tests
|
||||
b3Vector3 m_rayDirectionInverse;
|
||||
unsigned int m_signs[3];
|
||||
b3Scalar m_lambda_max;
|
||||
|
||||
virtual ~b3BroadphaseRayCallback() {}
|
||||
};
|
||||
|
||||
#endif //B3_BROADPHASE_CALLBACK_H
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -1,808 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
///b3DynamicBvhBroadphase implementation by Nathanael Presson
|
||||
|
||||
#include "b3DynamicBvhBroadphase.h"
|
||||
#include "b3OverlappingPair.h"
|
||||
|
||||
//
|
||||
// Profiling
|
||||
//
|
||||
|
||||
#if B3_DBVT_BP_PROFILE || B3_DBVT_BP_ENABLE_BENCHMARK
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
struct b3ProfileScope
|
||||
{
|
||||
__forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
|
||||
{
|
||||
}
|
||||
__forceinline ~b3ProfileScope()
|
||||
{
|
||||
(*m_value) += m_clock->getTimeMicroseconds() - m_base;
|
||||
}
|
||||
b3Clock* m_clock;
|
||||
unsigned long* m_value;
|
||||
unsigned long m_base;
|
||||
};
|
||||
#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock, _value_)
|
||||
#else
|
||||
#define b3SPC(_value_)
|
||||
#endif
|
||||
|
||||
//
|
||||
// Helpers
|
||||
//
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline void b3ListAppend(T* item, T*& list)
|
||||
{
|
||||
item->links[0] = 0;
|
||||
item->links[1] = list;
|
||||
if (list) list->links[0] = item;
|
||||
list = item;
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline void b3ListRemove(T* item, T*& list)
|
||||
{
|
||||
if (item->links[0])
|
||||
item->links[0]->links[1] = item->links[1];
|
||||
else
|
||||
list = item->links[1];
|
||||
if (item->links[1]) item->links[1]->links[0] = item->links[0];
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline int b3ListCount(T* root)
|
||||
{
|
||||
int n = 0;
|
||||
while (root)
|
||||
{
|
||||
++n;
|
||||
root = root->links[1];
|
||||
}
|
||||
return (n);
|
||||
}
|
||||
|
||||
//
|
||||
template <typename T>
|
||||
static inline void b3Clear(T& value)
|
||||
{
|
||||
static const struct ZeroDummy : T
|
||||
{
|
||||
} zerodummy;
|
||||
value = zerodummy;
|
||||
}
|
||||
|
||||
//
|
||||
// Colliders
|
||||
//
|
||||
|
||||
/* Tree collider */
|
||||
struct b3DbvtTreeCollider : b3DynamicBvh::ICollide
|
||||
{
|
||||
b3DynamicBvhBroadphase* pbp;
|
||||
b3DbvtProxy* proxy;
|
||||
b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
|
||||
void Process(const b3DbvtNode* na, const b3DbvtNode* nb)
|
||||
{
|
||||
if (na != nb)
|
||||
{
|
||||
b3DbvtProxy* pa = (b3DbvtProxy*)na->data;
|
||||
b3DbvtProxy* pb = (b3DbvtProxy*)nb->data;
|
||||
#if B3_DBVT_BP_SORTPAIRS
|
||||
if (pa->m_uniqueId > pb->m_uniqueId)
|
||||
b3Swap(pa, pb);
|
||||
#endif
|
||||
pbp->m_paircache->addOverlappingPair(pa->getUid(), pb->getUid());
|
||||
++pbp->m_newpairs;
|
||||
}
|
||||
}
|
||||
void Process(const b3DbvtNode* n)
|
||||
{
|
||||
Process(n, proxy->leaf);
|
||||
}
|
||||
};
|
||||
|
||||
//
|
||||
// b3DynamicBvhBroadphase
|
||||
//
|
||||
|
||||
//
|
||||
b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache)
|
||||
{
|
||||
m_deferedcollide = false;
|
||||
m_needcleanup = true;
|
||||
m_releasepaircache = (paircache != 0) ? false : true;
|
||||
m_prediction = 0;
|
||||
m_stageCurrent = 0;
|
||||
m_fixedleft = 0;
|
||||
m_fupdates = 1;
|
||||
m_dupdates = 0;
|
||||
m_cupdates = 10;
|
||||
m_newpairs = 1;
|
||||
m_updates_call = 0;
|
||||
m_updates_done = 0;
|
||||
m_updates_ratio = 0;
|
||||
m_paircache = paircache ? paircache : new (b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache), 16)) b3HashedOverlappingPairCache();
|
||||
|
||||
m_pid = 0;
|
||||
m_cid = 0;
|
||||
for (int i = 0; i <= STAGECOUNT; ++i)
|
||||
{
|
||||
m_stageRoots[i] = 0;
|
||||
}
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
b3Clear(m_profiling);
|
||||
#endif
|
||||
m_proxies.resize(proxyCapacity);
|
||||
}
|
||||
|
||||
//
|
||||
b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
|
||||
{
|
||||
if (m_releasepaircache)
|
||||
{
|
||||
m_paircache->~b3OverlappingPairCache();
|
||||
b3AlignedFree(m_paircache);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy(const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
int objectId,
|
||||
void* userPtr,
|
||||
int collisionFilterGroup,
|
||||
int collisionFilterMask)
|
||||
{
|
||||
b3DbvtProxy* mem = &m_proxies[objectId];
|
||||
b3DbvtProxy* proxy = new (mem) b3DbvtProxy(aabbMin, aabbMax, userPtr,
|
||||
collisionFilterGroup,
|
||||
collisionFilterMask);
|
||||
|
||||
b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
|
||||
|
||||
//bproxy->aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax);
|
||||
proxy->stage = m_stageCurrent;
|
||||
proxy->m_uniqueId = objectId;
|
||||
proxy->leaf = m_sets[0].insert(aabb, proxy);
|
||||
b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
|
||||
if (!m_deferedcollide)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
collider.proxy = proxy;
|
||||
m_sets[0].collideTV(m_sets[0].m_root, aabb, collider);
|
||||
m_sets[1].collideTV(m_sets[1].m_root, aabb, collider);
|
||||
}
|
||||
return (proxy);
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::destroyProxy(b3BroadphaseProxy* absproxy,
|
||||
b3Dispatcher* dispatcher)
|
||||
{
|
||||
b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
|
||||
if (proxy->stage == STAGECOUNT)
|
||||
m_sets[1].remove(proxy->leaf);
|
||||
else
|
||||
m_sets[0].remove(proxy->leaf);
|
||||
b3ListRemove(proxy, m_stageRoots[proxy->stage]);
|
||||
m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(), dispatcher);
|
||||
|
||||
m_needcleanup = true;
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const
|
||||
{
|
||||
const b3DbvtProxy* proxy = &m_proxies[objectId];
|
||||
aabbMin = proxy->m_aabbMin;
|
||||
aabbMax = proxy->m_aabbMax;
|
||||
}
|
||||
/*
|
||||
void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
|
||||
{
|
||||
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
aabbMin = proxy->m_aabbMin;
|
||||
aabbMax = proxy->m_aabbMax;
|
||||
}
|
||||
*/
|
||||
|
||||
struct BroadphaseRayTester : b3DynamicBvh::ICollide
|
||||
{
|
||||
b3BroadphaseRayCallback& m_rayCallback;
|
||||
BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
|
||||
: m_rayCallback(orgCallback)
|
||||
{
|
||||
}
|
||||
void Process(const b3DbvtNode* leaf)
|
||||
{
|
||||
b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
|
||||
m_rayCallback.process(proxy);
|
||||
}
|
||||
};
|
||||
|
||||
void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax)
|
||||
{
|
||||
BroadphaseRayTester callback(rayCallback);
|
||||
|
||||
m_sets[0].rayTestInternal(m_sets[0].m_root,
|
||||
rayFrom,
|
||||
rayTo,
|
||||
rayCallback.m_rayDirectionInverse,
|
||||
rayCallback.m_signs,
|
||||
rayCallback.m_lambda_max,
|
||||
aabbMin,
|
||||
aabbMax,
|
||||
callback);
|
||||
|
||||
m_sets[1].rayTestInternal(m_sets[1].m_root,
|
||||
rayFrom,
|
||||
rayTo,
|
||||
rayCallback.m_rayDirectionInverse,
|
||||
rayCallback.m_signs,
|
||||
rayCallback.m_lambda_max,
|
||||
aabbMin,
|
||||
aabbMax,
|
||||
callback);
|
||||
}
|
||||
|
||||
struct BroadphaseAabbTester : b3DynamicBvh::ICollide
|
||||
{
|
||||
b3BroadphaseAabbCallback& m_aabbCallback;
|
||||
BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
|
||||
: m_aabbCallback(orgCallback)
|
||||
{
|
||||
}
|
||||
void Process(const b3DbvtNode* leaf)
|
||||
{
|
||||
b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
|
||||
m_aabbCallback.process(proxy);
|
||||
}
|
||||
};
|
||||
|
||||
void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& aabbCallback)
|
||||
{
|
||||
BroadphaseAabbTester callback(aabbCallback);
|
||||
|
||||
const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds = b3DbvtVolume::FromMM(aabbMin, aabbMax);
|
||||
//process all children, that overlap with the given AABB bounds
|
||||
m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
|
||||
m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::setAabb(int objectId,
|
||||
const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
b3DbvtProxy* proxy = &m_proxies[objectId];
|
||||
// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
|
||||
aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
|
||||
#if B3_DBVT_BP_PREVENTFALSEUPDATE
|
||||
if (b3NotEqual(aabb, proxy->leaf->volume))
|
||||
#endif
|
||||
{
|
||||
bool docollide = false;
|
||||
if (proxy->stage == STAGECOUNT)
|
||||
{ /* fixed -> dynamic set */
|
||||
m_sets[1].remove(proxy->leaf);
|
||||
proxy->leaf = m_sets[0].insert(aabb, proxy);
|
||||
docollide = true;
|
||||
}
|
||||
else
|
||||
{ /* dynamic set */
|
||||
++m_updates_call;
|
||||
if (b3Intersect(proxy->leaf->volume, aabb))
|
||||
{ /* Moving */
|
||||
|
||||
const b3Vector3 delta = aabbMin - proxy->m_aabbMin;
|
||||
b3Vector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
|
||||
if (delta[0] < 0) velocity[0] = -velocity[0];
|
||||
if (delta[1] < 0) velocity[1] = -velocity[1];
|
||||
if (delta[2] < 0) velocity[2] = -velocity[2];
|
||||
if (
|
||||
#ifdef B3_DBVT_BP_MARGIN
|
||||
m_sets[0].update(proxy->leaf, aabb, velocity, B3_DBVT_BP_MARGIN)
|
||||
#else
|
||||
m_sets[0].update(proxy->leaf, aabb, velocity)
|
||||
#endif
|
||||
)
|
||||
{
|
||||
++m_updates_done;
|
||||
docollide = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{ /* Teleporting */
|
||||
m_sets[0].update(proxy->leaf, aabb);
|
||||
++m_updates_done;
|
||||
docollide = true;
|
||||
}
|
||||
}
|
||||
b3ListRemove(proxy, m_stageRoots[proxy->stage]);
|
||||
proxy->m_aabbMin = aabbMin;
|
||||
proxy->m_aabbMax = aabbMax;
|
||||
proxy->stage = m_stageCurrent;
|
||||
b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
|
||||
if (docollide)
|
||||
{
|
||||
m_needcleanup = true;
|
||||
if (!m_deferedcollide)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::setAabbForceUpdate(b3BroadphaseProxy* absproxy,
|
||||
const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
|
||||
aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
|
||||
bool docollide = false;
|
||||
if (proxy->stage == STAGECOUNT)
|
||||
{ /* fixed -> dynamic set */
|
||||
m_sets[1].remove(proxy->leaf);
|
||||
proxy->leaf = m_sets[0].insert(aabb, proxy);
|
||||
docollide = true;
|
||||
}
|
||||
else
|
||||
{ /* dynamic set */
|
||||
++m_updates_call;
|
||||
/* Teleporting */
|
||||
m_sets[0].update(proxy->leaf, aabb);
|
||||
++m_updates_done;
|
||||
docollide = true;
|
||||
}
|
||||
b3ListRemove(proxy, m_stageRoots[proxy->stage]);
|
||||
proxy->m_aabbMin = aabbMin;
|
||||
proxy->m_aabbMax = aabbMax;
|
||||
proxy->stage = m_stageCurrent;
|
||||
b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
|
||||
if (docollide)
|
||||
{
|
||||
m_needcleanup = true;
|
||||
if (!m_deferedcollide)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
collide(dispatcher);
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
if (0 == (m_pid % B3_DBVT_BP_PROFILING_RATE))
|
||||
{
|
||||
printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
|
||||
unsigned int total = m_profiling.m_total;
|
||||
if (total <= 0) total = 1;
|
||||
printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("cleanup: %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("total: %uus\r\n", total / B3_DBVT_BP_PROFILING_RATE);
|
||||
const unsigned long sum = m_profiling.m_ddcollide +
|
||||
m_profiling.m_fdcollide +
|
||||
m_profiling.m_cleanup;
|
||||
printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / B3_DBVT_BP_PROFILING_RATE);
|
||||
printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * B3_DBVT_BP_PROFILING_RATE));
|
||||
b3Clear(m_profiling);
|
||||
m_clock.reset();
|
||||
}
|
||||
#endif
|
||||
|
||||
performDeferredRemoval(dispatcher);
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
|
||||
{
|
||||
if (m_paircache->hasDeferredRemoval())
|
||||
{
|
||||
b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
|
||||
|
||||
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
|
||||
|
||||
int invalidPair = 0;
|
||||
|
||||
int i;
|
||||
|
||||
b3BroadphasePair previousPair = b3MakeBroadphasePair(-1, -1);
|
||||
|
||||
for (i = 0; i < overlappingPairArray.size(); i++)
|
||||
{
|
||||
b3BroadphasePair& pair = overlappingPairArray[i];
|
||||
|
||||
bool isDuplicate = (pair == previousPair);
|
||||
|
||||
previousPair = pair;
|
||||
|
||||
bool needsRemoval = false;
|
||||
|
||||
if (!isDuplicate)
|
||||
{
|
||||
//important to perform AABB check that is consistent with the broadphase
|
||||
b3DbvtProxy* pa = &m_proxies[pair.x];
|
||||
b3DbvtProxy* pb = &m_proxies[pair.y];
|
||||
bool hasOverlap = b3Intersect(pa->leaf->volume, pb->leaf->volume);
|
||||
|
||||
if (hasOverlap)
|
||||
{
|
||||
needsRemoval = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
needsRemoval = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//remove duplicate
|
||||
needsRemoval = true;
|
||||
//should have no algorithm
|
||||
}
|
||||
|
||||
if (needsRemoval)
|
||||
{
|
||||
m_paircache->cleanOverlappingPair(pair, dispatcher);
|
||||
|
||||
pair.x = -1;
|
||||
pair.y = -1;
|
||||
invalidPair++;
|
||||
}
|
||||
}
|
||||
|
||||
//perform a sort, to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
|
||||
overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
|
||||
{
|
||||
/*printf("---------------------------------------------------------\n");
|
||||
printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
|
||||
printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
|
||||
printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
|
||||
{
|
||||
printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
|
||||
getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
*/
|
||||
|
||||
b3SPC(m_profiling.m_total);
|
||||
/* optimize */
|
||||
m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
|
||||
if (m_fixedleft)
|
||||
{
|
||||
const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
|
||||
m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
|
||||
m_fixedleft = b3Max<int>(0, m_fixedleft - count);
|
||||
}
|
||||
/* dynamic -> fixed set */
|
||||
m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
|
||||
b3DbvtProxy* current = m_stageRoots[m_stageCurrent];
|
||||
if (current)
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
do
|
||||
{
|
||||
b3DbvtProxy* next = current->links[1];
|
||||
b3ListRemove(current, m_stageRoots[current->stage]);
|
||||
b3ListAppend(current, m_stageRoots[STAGECOUNT]);
|
||||
#if B3_DBVT_BP_ACCURATESLEEPING
|
||||
m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
|
||||
collider.proxy = current;
|
||||
b3DynamicBvh::collideTV(m_sets[0].m_root, current->aabb, collider);
|
||||
b3DynamicBvh::collideTV(m_sets[1].m_root, current->aabb, collider);
|
||||
#endif
|
||||
m_sets[0].remove(current->leaf);
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
|
||||
curAabb = b3DbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
|
||||
current->leaf = m_sets[1].insert(curAabb, current);
|
||||
current->stage = STAGECOUNT;
|
||||
current = next;
|
||||
} while (current);
|
||||
m_fixedleft = m_sets[1].m_leaves;
|
||||
m_needcleanup = true;
|
||||
}
|
||||
/* collide dynamics */
|
||||
{
|
||||
b3DbvtTreeCollider collider(this);
|
||||
if (m_deferedcollide)
|
||||
{
|
||||
b3SPC(m_profiling.m_fdcollide);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
|
||||
}
|
||||
if (m_deferedcollide)
|
||||
{
|
||||
b3SPC(m_profiling.m_ddcollide);
|
||||
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
|
||||
}
|
||||
}
|
||||
/* clean up */
|
||||
if (m_needcleanup)
|
||||
{
|
||||
b3SPC(m_profiling.m_cleanup);
|
||||
b3BroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
|
||||
if (pairs.size() > 0)
|
||||
{
|
||||
int ni = b3Min(pairs.size(), b3Max<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
|
||||
for (int i = 0; i < ni; ++i)
|
||||
{
|
||||
b3BroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
|
||||
b3DbvtProxy* pa = &m_proxies[p.x];
|
||||
b3DbvtProxy* pb = &m_proxies[p.y];
|
||||
if (!b3Intersect(pa->leaf->volume, pb->leaf->volume))
|
||||
{
|
||||
#if B3_DBVT_BP_SORTPAIRS
|
||||
if (pa->m_uniqueId > pb->m_uniqueId)
|
||||
b3Swap(pa, pb);
|
||||
#endif
|
||||
m_paircache->removeOverlappingPair(pa->getUid(), pb->getUid(), dispatcher);
|
||||
--ni;
|
||||
--i;
|
||||
}
|
||||
}
|
||||
if (pairs.size() > 0)
|
||||
m_cid = (m_cid + ni) % pairs.size();
|
||||
else
|
||||
m_cid = 0;
|
||||
}
|
||||
}
|
||||
++m_pid;
|
||||
m_newpairs = 1;
|
||||
m_needcleanup = false;
|
||||
if (m_updates_call > 0)
|
||||
{
|
||||
m_updates_ratio = m_updates_done / (b3Scalar)m_updates_call;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_updates_ratio = 0;
|
||||
}
|
||||
m_updates_done /= 2;
|
||||
m_updates_call /= 2;
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::optimize()
|
||||
{
|
||||
m_sets[0].optimizeTopDown();
|
||||
m_sets[1].optimizeTopDown();
|
||||
}
|
||||
|
||||
//
|
||||
b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
|
||||
{
|
||||
return (m_paircache);
|
||||
}
|
||||
|
||||
//
|
||||
const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
|
||||
{
|
||||
return (m_paircache);
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const
|
||||
{
|
||||
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
|
||||
bounds;
|
||||
|
||||
if (!m_sets[0].empty())
|
||||
if (!m_sets[1].empty())
|
||||
b3Merge(m_sets[0].m_root->volume,
|
||||
m_sets[1].m_root->volume, bounds);
|
||||
else
|
||||
bounds = m_sets[0].m_root->volume;
|
||||
else if (!m_sets[1].empty())
|
||||
bounds = m_sets[1].m_root->volume;
|
||||
else
|
||||
bounds = b3DbvtVolume::FromCR(b3MakeVector3(0, 0, 0), 0);
|
||||
aabbMin = bounds.Mins();
|
||||
aabbMax = bounds.Maxs();
|
||||
}
|
||||
|
||||
void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
|
||||
{
|
||||
int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
|
||||
if (!totalObjects)
|
||||
{
|
||||
//reset internal dynamic tree data structures
|
||||
m_sets[0].clear();
|
||||
m_sets[1].clear();
|
||||
|
||||
m_deferedcollide = false;
|
||||
m_needcleanup = true;
|
||||
m_stageCurrent = 0;
|
||||
m_fixedleft = 0;
|
||||
m_fupdates = 1;
|
||||
m_dupdates = 0;
|
||||
m_cupdates = 10;
|
||||
m_newpairs = 1;
|
||||
m_updates_call = 0;
|
||||
m_updates_done = 0;
|
||||
m_updates_ratio = 0;
|
||||
|
||||
m_pid = 0;
|
||||
m_cid = 0;
|
||||
for (int i = 0; i <= STAGECOUNT; ++i)
|
||||
{
|
||||
m_stageRoots[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
void b3DynamicBvhBroadphase::printStats()
|
||||
{
|
||||
}
|
||||
|
||||
//
|
||||
#if B3_DBVT_BP_ENABLE_BENCHMARK
|
||||
|
||||
struct b3BroadphaseBenchmark
|
||||
{
|
||||
struct Experiment
|
||||
{
|
||||
const char* name;
|
||||
int object_count;
|
||||
int update_count;
|
||||
int spawn_count;
|
||||
int iterations;
|
||||
b3Scalar speed;
|
||||
b3Scalar amplitude;
|
||||
};
|
||||
struct Object
|
||||
{
|
||||
b3Vector3 center;
|
||||
b3Vector3 extents;
|
||||
b3BroadphaseProxy* proxy;
|
||||
b3Scalar time;
|
||||
void update(b3Scalar speed, b3Scalar amplitude, b3BroadphaseInterface* pbi)
|
||||
{
|
||||
time += speed;
|
||||
center[0] = b3Cos(time * (b3Scalar)2.17) * amplitude +
|
||||
b3Sin(time) * amplitude / 2;
|
||||
center[1] = b3Cos(time * (b3Scalar)1.38) * amplitude +
|
||||
b3Sin(time) * amplitude;
|
||||
center[2] = b3Sin(time * (b3Scalar)0.777) * amplitude;
|
||||
pbi->setAabb(proxy, center - extents, center + extents, 0);
|
||||
}
|
||||
};
|
||||
static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
|
||||
static b3Scalar UnitRand() { return (UnsignedRand(16384) / (b3Scalar)16384); }
|
||||
static void OutputTime(const char* name, b3Clock& c, unsigned count = 0)
|
||||
{
|
||||
const unsigned long us = c.getTimeMicroseconds();
|
||||
const unsigned long ms = (us + 500) / 1000;
|
||||
const b3Scalar sec = us / (b3Scalar)(1000 * 1000);
|
||||
if (count > 0)
|
||||
printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
|
||||
else
|
||||
printf("%s : %u us (%u ms)\r\n", name, us, ms);
|
||||
}
|
||||
};
|
||||
|
||||
void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
|
||||
{
|
||||
static const b3BroadphaseBenchmark::Experiment experiments[] =
|
||||
{
|
||||
{"1024o.10%", 1024, 10, 0, 8192, (b3Scalar)0.005, (b3Scalar)100},
|
||||
/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
|
||||
{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
|
||||
};
|
||||
static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
|
||||
b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
|
||||
b3Clock wallclock;
|
||||
/* Begin */
|
||||
for (int iexp = 0; iexp < nexperiments; ++iexp)
|
||||
{
|
||||
const b3BroadphaseBenchmark::Experiment& experiment = experiments[iexp];
|
||||
const int object_count = experiment.object_count;
|
||||
const int update_count = (object_count * experiment.update_count) / 100;
|
||||
const int spawn_count = (object_count * experiment.spawn_count) / 100;
|
||||
const b3Scalar speed = experiment.speed;
|
||||
const b3Scalar amplitude = experiment.amplitude;
|
||||
printf("Experiment #%u '%s':\r\n", iexp, experiment.name);
|
||||
printf("\tObjects: %u\r\n", object_count);
|
||||
printf("\tUpdate: %u\r\n", update_count);
|
||||
printf("\tSpawn: %u\r\n", spawn_count);
|
||||
printf("\tSpeed: %f\r\n", speed);
|
||||
printf("\tAmplitude: %f\r\n", amplitude);
|
||||
srand(180673);
|
||||
/* Create objects */
|
||||
wallclock.reset();
|
||||
objects.reserve(object_count);
|
||||
for (int i = 0; i < object_count; ++i)
|
||||
{
|
||||
b3BroadphaseBenchmark::Object* po = new b3BroadphaseBenchmark::Object();
|
||||
po->center[0] = b3BroadphaseBenchmark::UnitRand() * 50;
|
||||
po->center[1] = b3BroadphaseBenchmark::UnitRand() * 50;
|
||||
po->center[2] = b3BroadphaseBenchmark::UnitRand() * 50;
|
||||
po->extents[0] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
|
||||
po->extents[1] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
|
||||
po->extents[2] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
|
||||
po->time = b3BroadphaseBenchmark::UnitRand() * 2000;
|
||||
po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
|
||||
objects.push_back(po);
|
||||
}
|
||||
b3BroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
|
||||
/* First update */
|
||||
wallclock.reset();
|
||||
for (int i = 0; i < objects.size(); ++i)
|
||||
{
|
||||
objects[i]->update(speed, amplitude, pbi);
|
||||
}
|
||||
b3BroadphaseBenchmark::OutputTime("\tFirst update", wallclock);
|
||||
/* Updates */
|
||||
wallclock.reset();
|
||||
for (int i = 0; i < experiment.iterations; ++i)
|
||||
{
|
||||
for (int j = 0; j < update_count; ++j)
|
||||
{
|
||||
objects[j]->update(speed, amplitude, pbi);
|
||||
}
|
||||
pbi->calculateOverlappingPairs(0);
|
||||
}
|
||||
b3BroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations);
|
||||
/* Clean up */
|
||||
wallclock.reset();
|
||||
for (int i = 0; i < objects.size(); ++i)
|
||||
{
|
||||
pbi->destroyProxy(objects[i]->proxy, 0);
|
||||
delete objects[i];
|
||||
}
|
||||
objects.resize(0);
|
||||
b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock);
|
||||
}
|
||||
}
|
||||
#else
|
||||
/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
|
||||
{}
|
||||
*/
|
||||
#endif
|
||||
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
#undef b3SPC
|
||||
#endif
|
||||
|
|
@ -1,197 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
///b3DynamicBvhBroadphase implementation by Nathanael Presson
|
||||
#ifndef B3_DBVT_BROADPHASE_H
|
||||
#define B3_DBVT_BROADPHASE_H
|
||||
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
#include "b3BroadphaseCallback.h"
|
||||
|
||||
//
|
||||
// Compile time config
|
||||
//
|
||||
|
||||
#define B3_DBVT_BP_PROFILE 0
|
||||
//#define B3_DBVT_BP_SORTPAIRS 1
|
||||
#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
|
||||
#define B3_DBVT_BP_ACCURATESLEEPING 0
|
||||
#define B3_DBVT_BP_ENABLE_BENCHMARK 0
|
||||
#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
|
||||
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
#define B3_DBVT_BP_PROFILING_RATE 256
|
||||
|
||||
#endif
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3BroadphaseProxy
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///optional filtering to cull potential collisions
|
||||
enum CollisionFilterGroups
|
||||
{
|
||||
DefaultFilter = 1,
|
||||
StaticFilter = 2,
|
||||
KinematicFilter = 4,
|
||||
DebrisFilter = 8,
|
||||
SensorTrigger = 16,
|
||||
CharacterFilter = 32,
|
||||
AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
|
||||
};
|
||||
|
||||
//Usually the client b3CollisionObject or Rigidbody class
|
||||
void* m_clientObject;
|
||||
int m_collisionFilterGroup;
|
||||
int m_collisionFilterMask;
|
||||
int m_uniqueId; //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
|
||||
|
||||
b3Vector3 m_aabbMin;
|
||||
b3Vector3 m_aabbMax;
|
||||
|
||||
B3_FORCE_INLINE int getUid() const
|
||||
{
|
||||
return m_uniqueId;
|
||||
}
|
||||
|
||||
//used for memory pools
|
||||
b3BroadphaseProxy() : m_clientObject(0)
|
||||
{
|
||||
}
|
||||
|
||||
b3BroadphaseProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
|
||||
: m_clientObject(userPtr),
|
||||
m_collisionFilterGroup(collisionFilterGroup),
|
||||
m_collisionFilterMask(collisionFilterMask),
|
||||
m_aabbMin(aabbMin),
|
||||
m_aabbMax(aabbMax)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
//
|
||||
// b3DbvtProxy
|
||||
//
|
||||
struct b3DbvtProxy : b3BroadphaseProxy
|
||||
{
|
||||
/* Fields */
|
||||
//b3DbvtAabbMm aabb;
|
||||
b3DbvtNode* leaf;
|
||||
b3DbvtProxy* links[2];
|
||||
int stage;
|
||||
/* ctor */
|
||||
|
||||
explicit b3DbvtProxy() {}
|
||||
b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
|
||||
{
|
||||
links[0] = links[1] = 0;
|
||||
}
|
||||
};
|
||||
|
||||
typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
|
||||
|
||||
///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
|
||||
///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
|
||||
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
|
||||
struct b3DynamicBvhBroadphase
|
||||
{
|
||||
/* Config */
|
||||
enum
|
||||
{
|
||||
DYNAMIC_SET = 0, /* Dynamic set index */
|
||||
FIXED_SET = 1, /* Fixed set index */
|
||||
STAGECOUNT = 2 /* Number of stages */
|
||||
};
|
||||
/* Fields */
|
||||
b3DynamicBvh m_sets[2]; // Dbvt sets
|
||||
b3DbvtProxy* m_stageRoots[STAGECOUNT + 1]; // Stages list
|
||||
|
||||
b3AlignedObjectArray<b3DbvtProxy> m_proxies;
|
||||
b3OverlappingPairCache* m_paircache; // Pair cache
|
||||
b3Scalar m_prediction; // Velocity prediction
|
||||
int m_stageCurrent; // Current stage
|
||||
int m_fupdates; // % of fixed updates per frame
|
||||
int m_dupdates; // % of dynamic updates per frame
|
||||
int m_cupdates; // % of cleanup updates per frame
|
||||
int m_newpairs; // Number of pairs created
|
||||
int m_fixedleft; // Fixed optimization left
|
||||
unsigned m_updates_call; // Number of updates call
|
||||
unsigned m_updates_done; // Number of updates done
|
||||
b3Scalar m_updates_ratio; // m_updates_done/m_updates_call
|
||||
int m_pid; // Parse id
|
||||
int m_cid; // Cleanup index
|
||||
bool m_releasepaircache; // Release pair cache on delete
|
||||
bool m_deferedcollide; // Defere dynamic/static collision to collide call
|
||||
bool m_needcleanup; // Need to run cleanup?
|
||||
#if B3_DBVT_BP_PROFILE
|
||||
b3Clock m_clock;
|
||||
struct
|
||||
{
|
||||
unsigned long m_total;
|
||||
unsigned long m_ddcollide;
|
||||
unsigned long m_fdcollide;
|
||||
unsigned long m_cleanup;
|
||||
unsigned long m_jobcount;
|
||||
} m_profiling;
|
||||
#endif
|
||||
/* Methods */
|
||||
b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache = 0);
|
||||
virtual ~b3DynamicBvhBroadphase();
|
||||
void collide(b3Dispatcher* dispatcher);
|
||||
void optimize();
|
||||
|
||||
/* b3BroadphaseInterface Implementation */
|
||||
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectIndex, void* userPtr, int collisionFilterGroup, int collisionFilterMask);
|
||||
virtual void destroyProxy(b3BroadphaseProxy* proxy, b3Dispatcher* dispatcher);
|
||||
virtual void setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* dispatcher);
|
||||
virtual void rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin = b3MakeVector3(0, 0, 0), const b3Vector3& aabbMax = b3MakeVector3(0, 0, 0));
|
||||
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
|
||||
|
||||
//virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
|
||||
virtual void getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const;
|
||||
virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher = 0);
|
||||
virtual b3OverlappingPairCache* getOverlappingPairCache();
|
||||
virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
|
||||
virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
|
||||
virtual void printStats();
|
||||
|
||||
///reset broadphase internal structures, to ensure determinism/reproducability
|
||||
virtual void resetPool(b3Dispatcher* dispatcher);
|
||||
|
||||
void performDeferredRemoval(b3Dispatcher* dispatcher);
|
||||
|
||||
void setVelocityPrediction(b3Scalar prediction)
|
||||
{
|
||||
m_prediction = prediction;
|
||||
}
|
||||
b3Scalar getVelocityPrediction() const
|
||||
{
|
||||
return m_prediction;
|
||||
}
|
||||
|
||||
///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
|
||||
///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
|
||||
///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
|
||||
///http://code.google.com/p/bullet/issues/detail?id=223
|
||||
void setAabbForceUpdate(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
|
||||
|
||||
//static void benchmark(b3BroadphaseInterface*);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_OVERLAPPING_PAIR_H
|
||||
#define B3_OVERLAPPING_PAIR_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
|
||||
#define B3_NEW_PAIR_MARKER -1
|
||||
#define B3_REMOVED_PAIR_MARKER -2
|
||||
|
||||
typedef b3Int4 b3BroadphasePair;
|
||||
|
||||
inline b3Int4 b3MakeBroadphasePair(int xx, int yy)
|
||||
{
|
||||
b3Int4 pair;
|
||||
|
||||
if (xx < yy)
|
||||
{
|
||||
pair.x = xx;
|
||||
pair.y = yy;
|
||||
}
|
||||
else
|
||||
{
|
||||
pair.x = yy;
|
||||
pair.y = xx;
|
||||
}
|
||||
pair.z = B3_NEW_PAIR_MARKER;
|
||||
pair.w = B3_NEW_PAIR_MARKER;
|
||||
return pair;
|
||||
}
|
||||
|
||||
/*struct b3BroadphasePair : public b3Int4
|
||||
{
|
||||
explicit b3BroadphasePair(){}
|
||||
|
||||
};
|
||||
*/
|
||||
|
||||
class b3BroadphasePairSortPredicate
|
||||
{
|
||||
public:
|
||||
bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
|
||||
{
|
||||
const int uidA0 = a.x;
|
||||
const int uidB0 = b.x;
|
||||
const int uidA1 = a.y;
|
||||
const int uidB1 = b.y;
|
||||
return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
|
||||
}
|
||||
};
|
||||
|
||||
B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
|
||||
{
|
||||
return (a.x == b.x) && (a.y == b.y);
|
||||
}
|
||||
|
||||
#endif //B3_OVERLAPPING_PAIR_H
|
||||
|
|
@ -1,559 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b3OverlappingPairCache.h"
|
||||
|
||||
//#include "b3Dispatcher.h"
|
||||
//#include "b3CollisionAlgorithm.h"
|
||||
#include "Bullet3Geometry/b3AabbUtil.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
int b3g_overlappingPairs = 0;
|
||||
int b3g_removePairs = 0;
|
||||
int b3g_addedPairs = 0;
|
||||
int b3g_findPairs = 0;
|
||||
|
||||
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
|
||||
//, m_blockedForChanges(false)
|
||||
{
|
||||
int initialAllocatedSize = 2;
|
||||
m_overlappingPairArray.reserve(initialAllocatedSize);
|
||||
growTables();
|
||||
}
|
||||
|
||||
b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
|
||||
{
|
||||
}
|
||||
|
||||
void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
|
||||
{
|
||||
/* if (pair.m_algorithm)
|
||||
{
|
||||
{
|
||||
pair.m_algorithm->~b3CollisionAlgorithm();
|
||||
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
|
||||
pair.m_algorithm=0;
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
|
||||
{
|
||||
class CleanPairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_cleanProxy;
|
||||
b3OverlappingPairCache* m_pairCache;
|
||||
b3Dispatcher* m_dispatcher;
|
||||
|
||||
public:
|
||||
CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
|
||||
: m_cleanProxy(cleanProxy),
|
||||
m_pairCache(pairCache),
|
||||
m_dispatcher(dispatcher)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
if ((pair.x == m_cleanProxy) ||
|
||||
(pair.y == m_cleanProxy))
|
||||
{
|
||||
m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
CleanPairCallback cleanPairs(proxy, this, dispatcher);
|
||||
|
||||
processAllOverlappingPairs(&cleanPairs, dispatcher);
|
||||
}
|
||||
|
||||
void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
|
||||
{
|
||||
class RemovePairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_obsoleteProxy;
|
||||
|
||||
public:
|
||||
RemovePairCallback(int obsoleteProxy)
|
||||
: m_obsoleteProxy(obsoleteProxy)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
return ((pair.x == m_obsoleteProxy) ||
|
||||
(pair.y == m_obsoleteProxy));
|
||||
}
|
||||
};
|
||||
|
||||
RemovePairCallback removeCallback(proxy);
|
||||
|
||||
processAllOverlappingPairs(&removeCallback, dispatcher);
|
||||
}
|
||||
|
||||
b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
|
||||
{
|
||||
b3g_findPairs++;
|
||||
if (proxy0 > proxy1)
|
||||
b3Swap(proxy0, proxy1);
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
|
||||
|
||||
if (hash >= m_hashTable.size())
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int index = m_hashTable[hash];
|
||||
while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
|
||||
{
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (index == B3_NULL_PAIR)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
b3Assert(index < m_overlappingPairArray.size());
|
||||
|
||||
return &m_overlappingPairArray[index];
|
||||
}
|
||||
|
||||
//#include <stdio.h>
|
||||
|
||||
void b3HashedOverlappingPairCache::growTables()
|
||||
{
|
||||
int newCapacity = m_overlappingPairArray.capacity();
|
||||
|
||||
if (m_hashTable.size() < newCapacity)
|
||||
{
|
||||
//grow hashtable and next table
|
||||
int curHashtableSize = m_hashTable.size();
|
||||
|
||||
m_hashTable.resize(newCapacity);
|
||||
m_next.resize(newCapacity);
|
||||
|
||||
int i;
|
||||
|
||||
for (i = 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_hashTable[i] = B3_NULL_PAIR;
|
||||
}
|
||||
for (i = 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_next[i] = B3_NULL_PAIR;
|
||||
}
|
||||
|
||||
for (i = 0; i < curHashtableSize; i++)
|
||||
{
|
||||
const b3BroadphasePair& pair = m_overlappingPairArray[i];
|
||||
int proxyId1 = pair.x;
|
||||
int proxyId2 = pair.y;
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
|
||||
m_next[i] = m_hashTable[hashValue];
|
||||
m_hashTable[hashValue] = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
|
||||
{
|
||||
if (proxy0 > proxy1)
|
||||
b3Swap(proxy0, proxy1);
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
|
||||
|
||||
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
|
||||
if (pair != NULL)
|
||||
{
|
||||
return pair;
|
||||
}
|
||||
/*for(int i=0;i<m_overlappingPairArray.size();++i)
|
||||
{
|
||||
if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
|
||||
(m_overlappingPairArray[i].m_pProxy1==proxy1))
|
||||
{
|
||||
printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
|
||||
internalFindPair(proxy0, proxy1, hash);
|
||||
}
|
||||
}*/
|
||||
int count = m_overlappingPairArray.size();
|
||||
int oldCapacity = m_overlappingPairArray.capacity();
|
||||
pair = &m_overlappingPairArray.expandNonInitializing();
|
||||
|
||||
//this is where we add an actual pair, so also call the 'ghost'
|
||||
// if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
|
||||
|
||||
int newCapacity = m_overlappingPairArray.capacity();
|
||||
|
||||
if (oldCapacity < newCapacity)
|
||||
{
|
||||
growTables();
|
||||
//hash with new capacity
|
||||
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
|
||||
}
|
||||
|
||||
*pair = b3MakeBroadphasePair(proxy0, proxy1);
|
||||
|
||||
// pair->m_pProxy0 = proxy0;
|
||||
// pair->m_pProxy1 = proxy1;
|
||||
//pair->m_algorithm = 0;
|
||||
//pair->m_internalTmpValue = 0;
|
||||
|
||||
m_next[count] = m_hashTable[hash];
|
||||
m_hashTable[hash] = count;
|
||||
|
||||
return pair;
|
||||
}
|
||||
|
||||
void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
|
||||
{
|
||||
b3g_removePairs++;
|
||||
if (proxy0 > proxy1)
|
||||
b3Swap(proxy0, proxy1);
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
|
||||
/*if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);*/
|
||||
|
||||
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
|
||||
|
||||
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
|
||||
if (pair == NULL)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
cleanOverlappingPair(*pair, dispatcher);
|
||||
|
||||
int pairIndex = int(pair - &m_overlappingPairArray[0]);
|
||||
b3Assert(pairIndex < m_overlappingPairArray.size());
|
||||
|
||||
// Remove the pair from the hash table.
|
||||
int index = m_hashTable[hash];
|
||||
b3Assert(index != B3_NULL_PAIR);
|
||||
|
||||
int previous = B3_NULL_PAIR;
|
||||
while (index != pairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_NULL_PAIR)
|
||||
{
|
||||
b3Assert(m_next[previous] == pairIndex);
|
||||
m_next[previous] = m_next[pairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[hash] = m_next[pairIndex];
|
||||
}
|
||||
|
||||
// We now move the last pair into spot of the
|
||||
// pair being removed. We need to fix the hash
|
||||
// table indices to support the move.
|
||||
|
||||
int lastPairIndex = m_overlappingPairArray.size() - 1;
|
||||
|
||||
//if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
|
||||
|
||||
// If the removed pair is the last pair, we are done.
|
||||
if (lastPairIndex == pairIndex)
|
||||
{
|
||||
m_overlappingPairArray.pop_back();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Remove the last pair from the hash table.
|
||||
const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
|
||||
/* missing swap here too, Nat. */
|
||||
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity() - 1));
|
||||
|
||||
index = m_hashTable[lastHash];
|
||||
b3Assert(index != B3_NULL_PAIR);
|
||||
|
||||
previous = B3_NULL_PAIR;
|
||||
while (index != lastPairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_NULL_PAIR)
|
||||
{
|
||||
b3Assert(m_next[previous] == lastPairIndex);
|
||||
m_next[previous] = m_next[lastPairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[lastHash] = m_next[lastPairIndex];
|
||||
}
|
||||
|
||||
// Copy the last pair into the remove pair's spot.
|
||||
m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
|
||||
|
||||
// Insert the last pair into the hash table
|
||||
m_next[pairIndex] = m_hashTable[lastHash];
|
||||
m_hashTable[lastHash] = pairIndex;
|
||||
|
||||
m_overlappingPairArray.pop_back();
|
||||
|
||||
return 0;
|
||||
}
|
||||
//#include <stdio.h>
|
||||
|
||||
void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
|
||||
{
|
||||
int i;
|
||||
|
||||
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
|
||||
for (i = 0; i < m_overlappingPairArray.size();)
|
||||
{
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray[i];
|
||||
if (callback->processOverlap(*pair))
|
||||
{
|
||||
removeOverlappingPair(pair->x, pair->y, dispatcher);
|
||||
|
||||
b3g_overlappingPairs--;
|
||||
}
|
||||
else
|
||||
{
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
///need to keep hashmap in sync with pair address, so rebuild all
|
||||
b3BroadphasePairArray tmpPairs;
|
||||
int i;
|
||||
for (i = 0; i < m_overlappingPairArray.size(); i++)
|
||||
{
|
||||
tmpPairs.push_back(m_overlappingPairArray[i]);
|
||||
}
|
||||
|
||||
for (i = 0; i < tmpPairs.size(); i++)
|
||||
{
|
||||
removeOverlappingPair(tmpPairs[i].x, tmpPairs[i].y, dispatcher);
|
||||
}
|
||||
|
||||
for (i = 0; i < m_next.size(); i++)
|
||||
{
|
||||
m_next[i] = B3_NULL_PAIR;
|
||||
}
|
||||
|
||||
tmpPairs.quickSort(b3BroadphasePairSortPredicate());
|
||||
|
||||
for (i = 0; i < tmpPairs.size(); i++)
|
||||
{
|
||||
addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
|
||||
}
|
||||
}
|
||||
|
||||
void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
|
||||
{
|
||||
if (!hasDeferredRemoval())
|
||||
{
|
||||
b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
|
||||
|
||||
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
|
||||
if (findIndex < m_overlappingPairArray.size())
|
||||
{
|
||||
b3g_overlappingPairs--;
|
||||
b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
|
||||
|
||||
cleanOverlappingPair(pair, dispatcher);
|
||||
//if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
|
||||
|
||||
m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
|
||||
m_overlappingPairArray.pop_back();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
|
||||
{
|
||||
//don't add overlap with own
|
||||
b3Assert(proxy0 != proxy1);
|
||||
|
||||
if (!needsBroadphaseCollision(proxy0, proxy1))
|
||||
return 0;
|
||||
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
|
||||
*pair = b3MakeBroadphasePair(proxy0, proxy1);
|
||||
|
||||
b3g_overlappingPairs++;
|
||||
b3g_addedPairs++;
|
||||
|
||||
// if (m_ghostPairCallback)
|
||||
// m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
|
||||
return pair;
|
||||
}
|
||||
|
||||
///this findPair becomes really slow. Either sort the list to speedup the query, or
|
||||
///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
|
||||
///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
|
||||
///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
|
||||
b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0, int proxy1)
|
||||
{
|
||||
if (!needsBroadphaseCollision(proxy0, proxy1))
|
||||
return 0;
|
||||
|
||||
b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0, proxy1);
|
||||
int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
|
||||
|
||||
if (findIndex < m_overlappingPairArray.size())
|
||||
{
|
||||
//b3Assert(it != m_overlappingPairSet.end());
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
|
||||
return pair;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//#include <stdio.h>
|
||||
|
||||
void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 0; i < m_overlappingPairArray.size();)
|
||||
{
|
||||
b3BroadphasePair* pair = &m_overlappingPairArray[i];
|
||||
if (callback->processOverlap(*pair))
|
||||
{
|
||||
cleanOverlappingPair(*pair, dispatcher);
|
||||
pair->x = -1;
|
||||
pair->y = -1;
|
||||
m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
|
||||
m_overlappingPairArray.pop_back();
|
||||
b3g_overlappingPairs--;
|
||||
}
|
||||
else
|
||||
{
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
|
||||
m_hasDeferredRemoval(true),
|
||||
m_overlapFilterCallback(0)
|
||||
|
||||
{
|
||||
int initialAllocatedSize = 2;
|
||||
m_overlappingPairArray.reserve(initialAllocatedSize);
|
||||
}
|
||||
|
||||
b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
|
||||
{
|
||||
}
|
||||
|
||||
void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
|
||||
{
|
||||
/* if (pair.m_algorithm)
|
||||
{
|
||||
{
|
||||
pair.m_algorithm->~b3CollisionAlgorithm();
|
||||
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
|
||||
pair.m_algorithm=0;
|
||||
b3g_removePairs--;
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
|
||||
{
|
||||
class CleanPairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_cleanProxy;
|
||||
b3OverlappingPairCache* m_pairCache;
|
||||
b3Dispatcher* m_dispatcher;
|
||||
|
||||
public:
|
||||
CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
|
||||
: m_cleanProxy(cleanProxy),
|
||||
m_pairCache(pairCache),
|
||||
m_dispatcher(dispatcher)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
if ((pair.x == m_cleanProxy) ||
|
||||
(pair.y == m_cleanProxy))
|
||||
{
|
||||
m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
CleanPairCallback cleanPairs(proxy, this, dispatcher);
|
||||
|
||||
processAllOverlappingPairs(&cleanPairs, dispatcher);
|
||||
}
|
||||
|
||||
void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
|
||||
{
|
||||
class RemovePairCallback : public b3OverlapCallback
|
||||
{
|
||||
int m_obsoleteProxy;
|
||||
|
||||
public:
|
||||
RemovePairCallback(int obsoleteProxy)
|
||||
: m_obsoleteProxy(obsoleteProxy)
|
||||
{
|
||||
}
|
||||
virtual bool processOverlap(b3BroadphasePair& pair)
|
||||
{
|
||||
return ((pair.x == m_obsoleteProxy) ||
|
||||
(pair.y == m_obsoleteProxy));
|
||||
}
|
||||
};
|
||||
|
||||
RemovePairCallback removeCallback(proxy);
|
||||
|
||||
processAllOverlappingPairs(&removeCallback, dispatcher);
|
||||
}
|
||||
|
||||
void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
//should already be sorted
|
||||
}
|
||||
|
|
@ -1,427 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_OVERLAPPING_PAIR_CACHE_H
|
||||
#define B3_OVERLAPPING_PAIR_CACHE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Int2.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
class b3Dispatcher;
|
||||
#include "b3OverlappingPair.h"
|
||||
|
||||
typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
|
||||
|
||||
struct b3OverlapCallback
|
||||
{
|
||||
virtual ~b3OverlapCallback()
|
||||
{
|
||||
}
|
||||
//return true for deletion of the pair
|
||||
virtual bool processOverlap(b3BroadphasePair& pair) = 0;
|
||||
};
|
||||
|
||||
struct b3OverlapFilterCallback
|
||||
{
|
||||
virtual ~b3OverlapFilterCallback()
|
||||
{
|
||||
}
|
||||
// return true when pairs need collision
|
||||
virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
|
||||
};
|
||||
|
||||
extern int b3g_removePairs;
|
||||
extern int b3g_addedPairs;
|
||||
extern int b3g_findPairs;
|
||||
|
||||
const int B3_NULL_PAIR = 0xffffffff;
|
||||
|
||||
///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
|
||||
///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
|
||||
class b3OverlappingPairCache
|
||||
{
|
||||
public:
|
||||
virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
|
||||
|
||||
virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
|
||||
|
||||
virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
|
||||
|
||||
virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
|
||||
|
||||
virtual void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
virtual int getNumOverlappingPairs() const = 0;
|
||||
|
||||
virtual void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher) = 0;
|
||||
|
||||
virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
|
||||
|
||||
virtual bool hasDeferredRemoval() = 0;
|
||||
|
||||
//virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
|
||||
|
||||
virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1) = 0;
|
||||
virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) = 0;
|
||||
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
|
||||
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
|
||||
};
|
||||
|
||||
/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
|
||||
class b3HashedOverlappingPairCache : public b3OverlappingPairCache
|
||||
{
|
||||
b3BroadphasePairArray m_overlappingPairArray;
|
||||
b3OverlapFilterCallback* m_overlapFilterCallback;
|
||||
// bool m_blockedForChanges;
|
||||
|
||||
public:
|
||||
b3HashedOverlappingPairCache();
|
||||
virtual ~b3HashedOverlappingPairCache();
|
||||
|
||||
virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
|
||||
|
||||
virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
|
||||
|
||||
B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0, int proxy1) const
|
||||
{
|
||||
if (m_overlapFilterCallback)
|
||||
return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
|
||||
|
||||
bool collides = true; //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
|
||||
//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
|
||||
|
||||
return collides;
|
||||
}
|
||||
|
||||
// Add a pair and return the new pair. If the pair already exists,
|
||||
// no new pair is created and the old one is returned.
|
||||
virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1)
|
||||
{
|
||||
b3g_addedPairs++;
|
||||
|
||||
if (!needsBroadphaseCollision(proxy0, proxy1))
|
||||
return 0;
|
||||
|
||||
return internalAddPair(proxy0, proxy1);
|
||||
}
|
||||
|
||||
void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
|
||||
|
||||
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
const b3BroadphasePair* getOverlappingPairArrayPtr() const
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
b3BroadphasePairArray& getOverlappingPairArray()
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
const b3BroadphasePairArray& getOverlappingPairArray() const
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
|
||||
|
||||
b3BroadphasePair* findPair(int proxy0, int proxy1);
|
||||
|
||||
int GetCount() const { return m_overlappingPairArray.size(); }
|
||||
// b3BroadphasePair* GetPairs() { return m_pairs; }
|
||||
|
||||
b3OverlapFilterCallback* getOverlapFilterCallback()
|
||||
{
|
||||
return m_overlapFilterCallback;
|
||||
}
|
||||
|
||||
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
|
||||
{
|
||||
m_overlapFilterCallback = callback;
|
||||
}
|
||||
|
||||
int getNumOverlappingPairs() const
|
||||
{
|
||||
return m_overlappingPairArray.size();
|
||||
}
|
||||
|
||||
private:
|
||||
b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
|
||||
|
||||
void growTables();
|
||||
|
||||
B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
|
||||
{
|
||||
return pair.x == proxyId1 && pair.y == proxyId2;
|
||||
}
|
||||
|
||||
/*
|
||||
// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
|
||||
// This assumes proxyId1 and proxyId2 are 16-bit.
|
||||
B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
|
||||
{
|
||||
int key = (proxyId2 << 16) | proxyId1;
|
||||
key = ~key + (key << 15);
|
||||
key = key ^ (key >> 12);
|
||||
key = key + (key << 2);
|
||||
key = key ^ (key >> 4);
|
||||
key = key * 2057;
|
||||
key = key ^ (key >> 16);
|
||||
return key;
|
||||
}
|
||||
*/
|
||||
|
||||
B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
|
||||
{
|
||||
int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) << 16));
|
||||
// Thomas Wang's hash
|
||||
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return static_cast<unsigned int>(key);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
|
||||
{
|
||||
int proxyId1 = proxy0;
|
||||
int proxyId2 = proxy1;
|
||||
#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
|
||||
if (proxyId1 > proxyId2)
|
||||
b3Swap(proxyId1, proxyId2);
|
||||
#endif
|
||||
|
||||
int index = m_hashTable[hash];
|
||||
|
||||
while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
|
||||
{
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (index == B3_NULL_PAIR)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
b3Assert(index < m_overlappingPairArray.size());
|
||||
|
||||
return &m_overlappingPairArray[index];
|
||||
}
|
||||
|
||||
virtual bool hasDeferredRemoval()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
|
||||
{
|
||||
m_ghostPairCallback = ghostPairCallback;
|
||||
}
|
||||
*/
|
||||
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
|
||||
|
||||
protected:
|
||||
b3AlignedObjectArray<int> m_hashTable;
|
||||
b3AlignedObjectArray<int> m_next;
|
||||
// b3OverlappingPairCallback* m_ghostPairCallback;
|
||||
};
|
||||
|
||||
///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
|
||||
///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
|
||||
class b3SortedOverlappingPairCache : public b3OverlappingPairCache
|
||||
{
|
||||
protected:
|
||||
//avoid brute-force finding all the time
|
||||
b3BroadphasePairArray m_overlappingPairArray;
|
||||
|
||||
//during the dispatch, check that user doesn't destroy/create proxy
|
||||
bool m_blockedForChanges;
|
||||
|
||||
///by default, do the removal during the pair traversal
|
||||
bool m_hasDeferredRemoval;
|
||||
|
||||
//if set, use the callback instead of the built in filter in needBroadphaseCollision
|
||||
b3OverlapFilterCallback* m_overlapFilterCallback;
|
||||
|
||||
// b3OverlappingPairCallback* m_ghostPairCallback;
|
||||
|
||||
public:
|
||||
b3SortedOverlappingPairCache();
|
||||
virtual ~b3SortedOverlappingPairCache();
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
|
||||
|
||||
void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
|
||||
|
||||
void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
|
||||
|
||||
b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1);
|
||||
|
||||
b3BroadphasePair* findPair(int proxy0, int proxy1);
|
||||
|
||||
void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
|
||||
|
||||
virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
|
||||
|
||||
inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
|
||||
{
|
||||
if (m_overlapFilterCallback)
|
||||
return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
|
||||
|
||||
bool collides = true; //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
|
||||
//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
|
||||
|
||||
return collides;
|
||||
}
|
||||
|
||||
b3BroadphasePairArray& getOverlappingPairArray()
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
const b3BroadphasePairArray& getOverlappingPairArray() const
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
b3BroadphasePair* getOverlappingPairArrayPtr()
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
const b3BroadphasePair* getOverlappingPairArrayPtr() const
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
|
||||
int getNumOverlappingPairs() const
|
||||
{
|
||||
return m_overlappingPairArray.size();
|
||||
}
|
||||
|
||||
b3OverlapFilterCallback* getOverlapFilterCallback()
|
||||
{
|
||||
return m_overlapFilterCallback;
|
||||
}
|
||||
|
||||
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
|
||||
{
|
||||
m_overlapFilterCallback = callback;
|
||||
}
|
||||
|
||||
virtual bool hasDeferredRemoval()
|
||||
{
|
||||
return m_hasDeferredRemoval;
|
||||
}
|
||||
|
||||
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
|
||||
{
|
||||
m_ghostPairCallback = ghostPairCallback;
|
||||
}
|
||||
*/
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
|
||||
};
|
||||
|
||||
///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
|
||||
class b3NullPairCache : public b3OverlappingPairCache
|
||||
{
|
||||
b3BroadphasePairArray m_overlappingPairArray;
|
||||
|
||||
public:
|
||||
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
const b3BroadphasePair* getOverlappingPairArrayPtr() const
|
||||
{
|
||||
return &m_overlappingPairArray[0];
|
||||
}
|
||||
b3BroadphasePairArray& getOverlappingPairArray()
|
||||
{
|
||||
return m_overlappingPairArray;
|
||||
}
|
||||
|
||||
virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual int getNumOverlappingPairs() const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual bool hasDeferredRemoval()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
|
||||
// {
|
||||
//
|
||||
// }
|
||||
|
||||
virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/, int /*proxy1*/)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void* removeOverlappingPair(int /*proxy0*/, int /*proxy1*/, b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
|
||||
{
|
||||
(void)dispatcher;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_OVERLAPPING_PAIR_CACHE_H
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
|
||||
#ifndef B3_AABB_H
|
||||
#define B3_AABB_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Mat3x3.h"
|
||||
|
||||
typedef struct b3Aabb b3Aabb_t;
|
||||
|
||||
struct b3Aabb
|
||||
{
|
||||
union {
|
||||
float m_min[4];
|
||||
b3Float4 m_minVec;
|
||||
int m_minIndices[4];
|
||||
};
|
||||
union {
|
||||
float m_max[4];
|
||||
b3Float4 m_maxVec;
|
||||
int m_signedMaxIndices[4];
|
||||
};
|
||||
};
|
||||
|
||||
inline void b3TransformAabb2(b3Float4ConstArg localAabbMin, b3Float4ConstArg localAabbMax, float margin,
|
||||
b3Float4ConstArg pos,
|
||||
b3QuatConstArg orn,
|
||||
b3Float4* aabbMinOut, b3Float4* aabbMaxOut)
|
||||
{
|
||||
b3Float4 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
|
||||
localHalfExtents += b3MakeFloat4(margin, margin, margin, 0.f);
|
||||
b3Float4 localCenter = 0.5f * (localAabbMax + localAabbMin);
|
||||
b3Mat3x3 m;
|
||||
m = b3QuatGetRotationMatrix(orn);
|
||||
b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
|
||||
b3Float4 center = b3TransformPoint(localCenter, pos, orn);
|
||||
|
||||
b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 0)),
|
||||
b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 1)),
|
||||
b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 2)),
|
||||
0.f);
|
||||
*aabbMinOut = center - extent;
|
||||
*aabbMaxOut = center + extent;
|
||||
}
|
||||
|
||||
/// conservative test for overlap between two aabbs
|
||||
inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1, b3Float4ConstArg aabbMax1,
|
||||
b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
|
||||
overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;
|
||||
overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;
|
||||
return overlap;
|
||||
}
|
||||
|
||||
#endif //B3_AABB_H
|
||||
|
|
@ -1,93 +0,0 @@
|
|||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
)
|
||||
|
||||
SET(Bullet3Collision_SRCS
|
||||
BroadPhaseCollision/b3DynamicBvh.cpp
|
||||
BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
|
||||
BroadPhaseCollision/b3OverlappingPairCache.cpp
|
||||
NarrowPhaseCollision/b3ConvexUtility.cpp
|
||||
NarrowPhaseCollision/b3CpuNarrowPhase.cpp
|
||||
)
|
||||
|
||||
SET(Bullet3CollisionBroadPhase_HDRS
|
||||
BroadPhaseCollision/b3BroadphaseCallback.h
|
||||
BroadPhaseCollision/b3DynamicBvh.h
|
||||
BroadPhaseCollision/b3DynamicBvhBroadphase.h
|
||||
BroadPhaseCollision/b3OverlappingPair.h
|
||||
BroadPhaseCollision/b3OverlappingPairCache.h
|
||||
)
|
||||
SET(Bullet3CollisionBroadPhaseShared_HDRS
|
||||
BroadPhaseCollision/shared/b3Aabb.h
|
||||
)
|
||||
|
||||
SET(Bullet3CollisionNarrowPhase_HDRS
|
||||
NarrowPhaseCollision/b3Config.h
|
||||
NarrowPhaseCollision/b3Contact4.h
|
||||
NarrowPhaseCollision/b3ConvexUtility.h
|
||||
NarrowPhaseCollision/b3CpuNarrowPhase.h
|
||||
NarrowPhaseCollision/b3RaycastInfo.h
|
||||
NarrowPhaseCollision/b3RigidBodyCL.h
|
||||
)
|
||||
SET(Bullet3CollisionNarrowPhaseShared_HDRS
|
||||
|
||||
NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
|
||||
NarrowPhaseCollision/shared/b3BvhTraversal.h
|
||||
NarrowPhaseCollision/shared/b3ClipFaces.h
|
||||
NarrowPhaseCollision/shared/b3Collidable.h
|
||||
NarrowPhaseCollision/shared/b3Contact4Data.h
|
||||
NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
|
||||
NarrowPhaseCollision/shared/b3ContactSphereSphere.h
|
||||
NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
|
||||
NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
|
||||
NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
|
||||
NarrowPhaseCollision/shared/b3MprPenetration.h
|
||||
NarrowPhaseCollision/shared/b3NewContactReduction.h
|
||||
NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
|
||||
NarrowPhaseCollision/shared/b3ReduceContacts.h
|
||||
NarrowPhaseCollision/shared/b3RigidBodyData.h
|
||||
NarrowPhaseCollision/shared/b3UpdateAabbs.h
|
||||
)
|
||||
|
||||
SET(Bullet3Collision_HDRS
|
||||
${Bullet3CollisionBroadPhase_HDRS}
|
||||
${Bullet3CollisionBroadPhaseShared_HDRS}
|
||||
${Bullet3CollisionNarrowPhaseShared_HDRS}
|
||||
${Bullet3CollisionNarrowPhase_HDRS}
|
||||
)
|
||||
|
||||
ADD_LIBRARY(Bullet3Collision ${Bullet3Collision_SRCS} ${Bullet3Collision_HDRS})
|
||||
if (BUILD_SHARED_LIBS)
|
||||
target_link_libraries(Bullet3Collision Bullet3Geometry)
|
||||
endif ()
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#FILES_MATCHING requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Collision DESTINATION .)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Collision
|
||||
RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
|
||||
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES FRAMEWORK true)
|
||||
SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES PUBLIC_HEADER "${Bullet3Collision_HDRS}")
|
||||
# Have to list out sub-directories manually:
|
||||
#todo
|
||||
#SET_PROPERTY(SOURCE ${Bullet3CollisionBroadPhase_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadPhaseCollision)
|
||||
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
#ifndef B3_CONFIG_H
|
||||
#define B3_CONFIG_H
|
||||
|
||||
struct b3Config
|
||||
{
|
||||
int m_maxConvexBodies;
|
||||
int m_maxConvexShapes;
|
||||
int m_maxBroadphasePairs;
|
||||
int m_maxContactCapacity;
|
||||
int m_compoundPairCapacity;
|
||||
|
||||
int m_maxVerticesPerFace;
|
||||
int m_maxFacesPerShape;
|
||||
int m_maxConvexVertices;
|
||||
int m_maxConvexIndices;
|
||||
int m_maxConvexUniqueEdges;
|
||||
|
||||
int m_maxCompoundChildShapes;
|
||||
|
||||
int m_maxTriConvexPairCapacity;
|
||||
|
||||
b3Config()
|
||||
: m_maxConvexBodies(128 * 1024),
|
||||
m_maxVerticesPerFace(64),
|
||||
m_maxFacesPerShape(12),
|
||||
m_maxConvexVertices(8192),
|
||||
m_maxConvexIndices(81920),
|
||||
m_maxConvexUniqueEdges(8192),
|
||||
m_maxCompoundChildShapes(8192),
|
||||
m_maxTriConvexPairCapacity(256 * 1024)
|
||||
{
|
||||
m_maxConvexShapes = m_maxConvexBodies;
|
||||
m_maxBroadphasePairs = 16 * m_maxConvexBodies;
|
||||
m_maxContactCapacity = m_maxBroadphasePairs;
|
||||
m_compoundPairCapacity = 1024 * 1024;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_CONFIG_H
|
||||
|
|
@ -1,55 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_CONTACT4_H
|
||||
#define B3_CONTACT4_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3Contact4 : public b3Contact4Data
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
|
||||
int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
|
||||
bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
|
||||
bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
|
||||
// todo. make it safer
|
||||
int& getBatchIdx() { return m_batchIdx; }
|
||||
const int& getBatchIdx() const { return m_batchIdx; }
|
||||
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
|
||||
void setRestituitionCoeff(float c)
|
||||
{
|
||||
b3Assert(c >= 0.f && c <= 1.f);
|
||||
m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
|
||||
}
|
||||
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
|
||||
void setFrictionCoeff(float c)
|
||||
{
|
||||
b3Assert(c >= 0.f && c <= 1.f);
|
||||
m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
|
||||
}
|
||||
|
||||
//float& getNPoints() { return m_worldNormal[3]; }
|
||||
int getNPoints() const { return (int)m_worldNormalOnB.w; }
|
||||
|
||||
float getPenetration(int idx) const { return m_worldPosB[idx].w; }
|
||||
|
||||
bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
|
||||
};
|
||||
|
||||
#endif //B3_CONTACT4_H
|
||||
|
|
@ -1,500 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#include "b3ConvexUtility.h"
|
||||
#include "Bullet3Geometry/b3ConvexHullComputer.h"
|
||||
#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3HashMap.h"
|
||||
|
||||
b3ConvexUtility::~b3ConvexUtility()
|
||||
{
|
||||
}
|
||||
|
||||
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
|
||||
{
|
||||
b3ConvexHullComputer conv;
|
||||
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> faceNormals;
|
||||
int numFaces = conv.faces.size();
|
||||
faceNormals.resize(numFaces);
|
||||
b3ConvexHullComputer* convexUtil = &conv;
|
||||
|
||||
b3AlignedObjectArray<b3MyFace> tmpFaces;
|
||||
tmpFaces.resize(numFaces);
|
||||
|
||||
int numVertices = convexUtil->vertices.size();
|
||||
m_vertices.resize(numVertices);
|
||||
for (int p = 0; p < numVertices; p++)
|
||||
{
|
||||
m_vertices[p] = convexUtil->vertices[p];
|
||||
}
|
||||
|
||||
for (int i = 0; i < numFaces; i++)
|
||||
{
|
||||
int face = convexUtil->faces[i];
|
||||
//printf("face=%d\n",face);
|
||||
const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
|
||||
const b3ConvexHullComputer::Edge* edge = firstEdge;
|
||||
|
||||
b3Vector3 edges[3];
|
||||
int numEdges = 0;
|
||||
//compute face normals
|
||||
|
||||
do
|
||||
{
|
||||
int src = edge->getSourceVertex();
|
||||
tmpFaces[i].m_indices.push_back(src);
|
||||
int targ = edge->getTargetVertex();
|
||||
b3Vector3 wa = convexUtil->vertices[src];
|
||||
|
||||
b3Vector3 wb = convexUtil->vertices[targ];
|
||||
b3Vector3 newEdge = wb - wa;
|
||||
newEdge.normalize();
|
||||
if (numEdges < 2)
|
||||
edges[numEdges++] = newEdge;
|
||||
|
||||
edge = edge->getNextEdgeOfFace();
|
||||
} while (edge != firstEdge);
|
||||
|
||||
b3Scalar planeEq = 1e30f;
|
||||
|
||||
if (numEdges == 2)
|
||||
{
|
||||
faceNormals[i] = edges[0].cross(edges[1]);
|
||||
faceNormals[i].normalize();
|
||||
tmpFaces[i].m_plane[0] = faceNormals[i].getX();
|
||||
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
|
||||
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
|
||||
tmpFaces[i].m_plane[3] = planeEq;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Assert(0); //degenerate?
|
||||
faceNormals[i].setZero();
|
||||
}
|
||||
|
||||
for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
|
||||
{
|
||||
b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
|
||||
if (planeEq > eq)
|
||||
{
|
||||
planeEq = eq;
|
||||
}
|
||||
}
|
||||
tmpFaces[i].m_plane[3] = -planeEq;
|
||||
}
|
||||
|
||||
//merge coplanar faces and copy them to m_polyhedron
|
||||
|
||||
b3Scalar faceWeldThreshold = 0.999f;
|
||||
b3AlignedObjectArray<int> todoFaces;
|
||||
for (int i = 0; i < tmpFaces.size(); i++)
|
||||
todoFaces.push_back(i);
|
||||
|
||||
while (todoFaces.size())
|
||||
{
|
||||
b3AlignedObjectArray<int> coplanarFaceGroup;
|
||||
int refFace = todoFaces[todoFaces.size() - 1];
|
||||
|
||||
coplanarFaceGroup.push_back(refFace);
|
||||
b3MyFace& faceA = tmpFaces[refFace];
|
||||
todoFaces.pop_back();
|
||||
|
||||
b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
|
||||
for (int j = todoFaces.size() - 1; j >= 0; j--)
|
||||
{
|
||||
int i = todoFaces[j];
|
||||
b3MyFace& faceB = tmpFaces[i];
|
||||
b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
|
||||
if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
|
||||
{
|
||||
coplanarFaceGroup.push_back(i);
|
||||
todoFaces.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
bool did_merge = false;
|
||||
if (coplanarFaceGroup.size() > 1)
|
||||
{
|
||||
//do the merge: use Graham Scan 2d convex hull
|
||||
|
||||
b3AlignedObjectArray<b3GrahamVector3> orgpoints;
|
||||
b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
|
||||
|
||||
for (int i = 0; i < coplanarFaceGroup.size(); i++)
|
||||
{
|
||||
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
|
||||
|
||||
b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
|
||||
b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
|
||||
averageFaceNormal += faceNormal;
|
||||
for (int f = 0; f < face.m_indices.size(); f++)
|
||||
{
|
||||
int orgIndex = face.m_indices[f];
|
||||
b3Vector3 pt = m_vertices[orgIndex];
|
||||
|
||||
bool found = false;
|
||||
|
||||
for (int i = 0; i < orgpoints.size(); i++)
|
||||
{
|
||||
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
|
||||
if (orgpoints[i].m_orgIndex == orgIndex)
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found)
|
||||
orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
|
||||
}
|
||||
}
|
||||
|
||||
b3MyFace combinedFace;
|
||||
for (int i = 0; i < 4; i++)
|
||||
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
|
||||
|
||||
b3AlignedObjectArray<b3GrahamVector3> hull;
|
||||
|
||||
averageFaceNormal.normalize();
|
||||
b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
|
||||
|
||||
for (int i = 0; i < hull.size(); i++)
|
||||
{
|
||||
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
|
||||
for (int k = 0; k < orgpoints.size(); k++)
|
||||
{
|
||||
if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
|
||||
{
|
||||
orgpoints[k].m_orgIndex = -1; // invalidate...
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// are there rejected vertices?
|
||||
bool reject_merge = false;
|
||||
|
||||
for (int i = 0; i < orgpoints.size(); i++)
|
||||
{
|
||||
if (orgpoints[i].m_orgIndex == -1)
|
||||
continue; // this is in the hull...
|
||||
// this vertex is rejected -- is anybody else using this vertex?
|
||||
for (int j = 0; j < tmpFaces.size(); j++)
|
||||
{
|
||||
b3MyFace& face = tmpFaces[j];
|
||||
// is this a face of the current coplanar group?
|
||||
bool is_in_current_group = false;
|
||||
for (int k = 0; k < coplanarFaceGroup.size(); k++)
|
||||
{
|
||||
if (coplanarFaceGroup[k] == j)
|
||||
{
|
||||
is_in_current_group = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (is_in_current_group) // ignore this face...
|
||||
continue;
|
||||
// does this face use this rejected vertex?
|
||||
for (int v = 0; v < face.m_indices.size(); v++)
|
||||
{
|
||||
if (face.m_indices[v] == orgpoints[i].m_orgIndex)
|
||||
{
|
||||
// this rejected vertex is used in another face -- reject merge
|
||||
reject_merge = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (reject_merge)
|
||||
break;
|
||||
}
|
||||
if (reject_merge)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!reject_merge)
|
||||
{
|
||||
// do this merge!
|
||||
did_merge = true;
|
||||
m_faces.push_back(combinedFace);
|
||||
}
|
||||
}
|
||||
if (!did_merge)
|
||||
{
|
||||
for (int i = 0; i < coplanarFaceGroup.size(); i++)
|
||||
{
|
||||
b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
|
||||
m_faces.push_back(face);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
initialize();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
inline bool IsAlmostZero(const b3Vector3& v)
|
||||
{
|
||||
if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
struct b3InternalVertexPair
|
||||
{
|
||||
b3InternalVertexPair(short int v0, short int v1)
|
||||
: m_v0(v0),
|
||||
m_v1(v1)
|
||||
{
|
||||
if (m_v1 > m_v0)
|
||||
b3Swap(m_v0, m_v1);
|
||||
}
|
||||
short int m_v0;
|
||||
short int m_v1;
|
||||
int getHash() const
|
||||
{
|
||||
return m_v0 + (m_v1 << 16);
|
||||
}
|
||||
bool equals(const b3InternalVertexPair& other) const
|
||||
{
|
||||
return m_v0 == other.m_v0 && m_v1 == other.m_v1;
|
||||
}
|
||||
};
|
||||
|
||||
struct b3InternalEdge
|
||||
{
|
||||
b3InternalEdge()
|
||||
: m_face0(-1),
|
||||
m_face1(-1)
|
||||
{
|
||||
}
|
||||
short int m_face0;
|
||||
short int m_face1;
|
||||
};
|
||||
|
||||
//
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
bool b3ConvexUtility::testContainment() const
|
||||
{
|
||||
for (int p = 0; p < 8; p++)
|
||||
{
|
||||
b3Vector3 LocalPt;
|
||||
if (p == 0)
|
||||
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
|
||||
else if (p == 1)
|
||||
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
|
||||
else if (p == 2)
|
||||
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
|
||||
else if (p == 3)
|
||||
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
|
||||
else if (p == 4)
|
||||
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
|
||||
else if (p == 5)
|
||||
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
|
||||
else if (p == 6)
|
||||
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
|
||||
else if (p == 7)
|
||||
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
|
||||
|
||||
for (int i = 0; i < m_faces.size(); i++)
|
||||
{
|
||||
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
|
||||
const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
|
||||
if (d > 0.0f)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
void b3ConvexUtility::initialize()
|
||||
{
|
||||
b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
|
||||
|
||||
b3Scalar TotalArea = 0.0f;
|
||||
|
||||
m_localCenter.setValue(0, 0, 0);
|
||||
for (int i = 0; i < m_faces.size(); i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
int NbTris = numVertices;
|
||||
for (int j = 0; j < NbTris; j++)
|
||||
{
|
||||
int k = (j + 1) % numVertices;
|
||||
b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
|
||||
b3InternalEdge* edptr = edges.find(vp);
|
||||
b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
|
||||
edge.normalize();
|
||||
|
||||
bool found = false;
|
||||
b3Vector3 diff, diff2;
|
||||
|
||||
for (int p = 0; p < m_uniqueEdges.size(); p++)
|
||||
{
|
||||
diff = m_uniqueEdges[p] - edge;
|
||||
diff2 = m_uniqueEdges[p] + edge;
|
||||
|
||||
// if ((diff.length2()==0.f) ||
|
||||
// (diff2.length2()==0.f))
|
||||
|
||||
if (IsAlmostZero(diff) ||
|
||||
IsAlmostZero(diff2))
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found)
|
||||
{
|
||||
m_uniqueEdges.push_back(edge);
|
||||
}
|
||||
|
||||
if (edptr)
|
||||
{
|
||||
//TBD: figure out why I added this assert
|
||||
// b3Assert(edptr->m_face0>=0);
|
||||
// b3Assert(edptr->m_face1<0);
|
||||
edptr->m_face1 = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3InternalEdge ed;
|
||||
ed.m_face0 = i;
|
||||
edges.insert(vp, ed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_CONNECTED_FACES
|
||||
for (int i = 0; i < m_faces.size(); i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
m_faces[i].m_connectedFaces.resize(numVertices);
|
||||
|
||||
for (int j = 0; j < numVertices; j++)
|
||||
{
|
||||
int k = (j + 1) % numVertices;
|
||||
b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
|
||||
b3InternalEdge* edptr = edges.find(vp);
|
||||
b3Assert(edptr);
|
||||
b3Assert(edptr->m_face0 >= 0);
|
||||
b3Assert(edptr->m_face1 >= 0);
|
||||
|
||||
int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
|
||||
m_faces[i].m_connectedFaces[j] = connectedFace;
|
||||
}
|
||||
}
|
||||
#endif //USE_CONNECTED_FACES
|
||||
|
||||
for (int i = 0; i < m_faces.size(); i++)
|
||||
{
|
||||
int numVertices = m_faces[i].m_indices.size();
|
||||
int NbTris = numVertices - 2;
|
||||
|
||||
const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
|
||||
for (int j = 1; j <= NbTris; j++)
|
||||
{
|
||||
int k = (j + 1) % numVertices;
|
||||
const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
|
||||
const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
|
||||
b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
|
||||
b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
|
||||
m_localCenter += Area * Center;
|
||||
TotalArea += Area;
|
||||
}
|
||||
}
|
||||
m_localCenter /= TotalArea;
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
if (1)
|
||||
{
|
||||
m_radius = FLT_MAX;
|
||||
for (int i = 0; i < m_faces.size(); i++)
|
||||
{
|
||||
const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
|
||||
const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
|
||||
if (dist < m_radius)
|
||||
m_radius = dist;
|
||||
}
|
||||
|
||||
b3Scalar MinX = FLT_MAX;
|
||||
b3Scalar MinY = FLT_MAX;
|
||||
b3Scalar MinZ = FLT_MAX;
|
||||
b3Scalar MaxX = -FLT_MAX;
|
||||
b3Scalar MaxY = -FLT_MAX;
|
||||
b3Scalar MaxZ = -FLT_MAX;
|
||||
for (int i = 0; i < m_vertices.size(); i++)
|
||||
{
|
||||
const b3Vector3& pt = m_vertices[i];
|
||||
if (pt.getX() < MinX) MinX = pt.getX();
|
||||
if (pt.getX() > MaxX) MaxX = pt.getX();
|
||||
if (pt.getY() < MinY) MinY = pt.getY();
|
||||
if (pt.getY() > MaxY) MaxY = pt.getY();
|
||||
if (pt.getZ() < MinZ) MinZ = pt.getZ();
|
||||
if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
|
||||
}
|
||||
mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
|
||||
mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
|
||||
|
||||
// const b3Scalar r = m_radius / sqrtf(2.0f);
|
||||
const b3Scalar r = m_radius / sqrtf(3.0f);
|
||||
const int LargestExtent = mE.maxAxis();
|
||||
const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
|
||||
m_extents[0] = m_extents[1] = m_extents[2] = r;
|
||||
m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
|
||||
bool FoundBox = false;
|
||||
for (int j = 0; j < 1024; j++)
|
||||
{
|
||||
if (testContainment())
|
||||
{
|
||||
FoundBox = true;
|
||||
break;
|
||||
}
|
||||
|
||||
m_extents[LargestExtent] -= Step;
|
||||
}
|
||||
if (!FoundBox)
|
||||
{
|
||||
m_extents[0] = m_extents[1] = m_extents[2] = r;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Refine the box
|
||||
const b3Scalar Step = (m_radius - r) / 1024.0f;
|
||||
const int e0 = (1 << LargestExtent) & 3;
|
||||
const int e1 = (1 << e0) & 3;
|
||||
|
||||
for (int j = 0; j < 1024; j++)
|
||||
{
|
||||
const b3Scalar Saved0 = m_extents[e0];
|
||||
const b3Scalar Saved1 = m_extents[e1];
|
||||
m_extents[e0] += Step;
|
||||
m_extents[e1] += Step;
|
||||
|
||||
if (!testContainment())
|
||||
{
|
||||
m_extents[e0] = Saved0;
|
||||
m_extents[e1] = Saved1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1,55 +0,0 @@
|
|||
|
||||
/*
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#ifndef _BT_CONVEX_UTILITY_H
|
||||
#define _BT_CONVEX_UTILITY_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
|
||||
struct b3MyFace
|
||||
{
|
||||
b3AlignedObjectArray<int> m_indices;
|
||||
b3Scalar m_plane[4];
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3ConvexUtility
|
||||
{
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3Vector3 m_localCenter;
|
||||
b3Vector3 m_extents;
|
||||
b3Vector3 mC;
|
||||
b3Vector3 mE;
|
||||
b3Scalar m_radius;
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> m_vertices;
|
||||
b3AlignedObjectArray<b3MyFace> m_faces;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
|
||||
b3ConvexUtility()
|
||||
{
|
||||
}
|
||||
virtual ~b3ConvexUtility();
|
||||
|
||||
bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
|
||||
|
||||
void initialize();
|
||||
bool testContainment() const;
|
||||
};
|
||||
#endif
|
||||
|
|
@ -1,297 +0,0 @@
|
|||
#include "b3CpuNarrowPhase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
|
||||
|
||||
struct b3CpuNarrowPhaseInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
|
||||
b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
|
||||
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
|
||||
b3Config m_config;
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
|
||||
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
|
||||
b3AlignedObjectArray<b3Vector3> m_convexVertices;
|
||||
b3AlignedObjectArray<int> m_convexIndices;
|
||||
b3AlignedObjectArray<b3GpuFace> m_convexFaces;
|
||||
|
||||
b3AlignedObjectArray<b3Contact4Data> m_contacts;
|
||||
|
||||
int m_numAcceleratedShapes;
|
||||
};
|
||||
|
||||
const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
|
||||
{
|
||||
return m_data->m_contacts;
|
||||
}
|
||||
|
||||
b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
|
||||
{
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
|
||||
{
|
||||
return m_data->m_collidablesCPU[collidableIndex];
|
||||
}
|
||||
|
||||
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
|
||||
{
|
||||
m_data = new b3CpuNarrowPhaseInternalData;
|
||||
m_data->m_config = config;
|
||||
m_data->m_numAcceleratedShapes = 0;
|
||||
}
|
||||
|
||||
b3CpuNarrowPhase::~b3CpuNarrowPhase()
|
||||
{
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
|
||||
{
|
||||
int nPairs = pairs.size();
|
||||
int numContacts = 0;
|
||||
int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
|
||||
m_data->m_contacts.resize(maxContactCapacity);
|
||||
|
||||
for (int i = 0; i < nPairs; i++)
|
||||
{
|
||||
int bodyIndexA = pairs[i].x;
|
||||
int bodyIndexB = pairs[i].y;
|
||||
int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
|
||||
{
|
||||
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
//printf("convex-sphere\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
|
||||
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
|
||||
// printf("convex-plane\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("convex-plane\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
|
||||
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
|
||||
// printf("plane-convex\n");
|
||||
}
|
||||
|
||||
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
|
||||
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
//printf("pairs[i].z=%d\n",pairs[i].z);
|
||||
//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
|
||||
// m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
||||
int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
|
||||
m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
|
||||
|
||||
if (contactIndex >= 0)
|
||||
{
|
||||
pairs[i].z = contactIndex;
|
||||
}
|
||||
// printf("plane-convex\n");
|
||||
}
|
||||
}
|
||||
|
||||
m_data->m_contacts.resize(numContacts);
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
|
||||
{
|
||||
int collidableIndex = allocateCollidable();
|
||||
if (collidableIndex < 0)
|
||||
return collidableIndex;
|
||||
|
||||
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
|
||||
col.m_shapeType = SHAPE_CONVEX_HULL;
|
||||
col.m_shapeIndex = -1;
|
||||
|
||||
{
|
||||
b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
|
||||
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
|
||||
localCenter += utilPtr->m_vertices[i];
|
||||
localCenter *= (1.f / utilPtr->m_vertices.size());
|
||||
utilPtr->m_localCenter = localCenter;
|
||||
|
||||
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
|
||||
}
|
||||
|
||||
if (col.m_shapeIndex >= 0)
|
||||
{
|
||||
b3Aabb aabb;
|
||||
|
||||
b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
|
||||
b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
|
||||
|
||||
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
|
||||
{
|
||||
myAabbMin.setMin(utilPtr->m_vertices[i]);
|
||||
myAabbMax.setMax(utilPtr->m_vertices[i]);
|
||||
}
|
||||
aabb.m_min[0] = myAabbMin[0];
|
||||
aabb.m_min[1] = myAabbMin[1];
|
||||
aabb.m_min[2] = myAabbMin[2];
|
||||
aabb.m_minIndices[3] = 0;
|
||||
|
||||
aabb.m_max[0] = myAabbMax[0];
|
||||
aabb.m_max[1] = myAabbMax[1];
|
||||
aabb.m_max[2] = myAabbMax[2];
|
||||
aabb.m_signedMaxIndices[3] = 0;
|
||||
|
||||
m_data->m_localShapeAABBCPU.push_back(aabb);
|
||||
}
|
||||
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::allocateCollidable()
|
||||
{
|
||||
int curSize = m_data->m_collidablesCPU.size();
|
||||
if (curSize < m_data->m_config.m_maxConvexShapes)
|
||||
{
|
||||
m_data->m_collidablesCPU.expand();
|
||||
return curSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
|
||||
{
|
||||
b3AlignedObjectArray<b3Vector3> verts;
|
||||
|
||||
unsigned char* vts = (unsigned char*)vertices;
|
||||
for (int i = 0; i < numVertices; i++)
|
||||
{
|
||||
float* vertex = (float*)&vts[i * strideInBytes];
|
||||
verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
|
||||
}
|
||||
|
||||
b3ConvexUtility* utilPtr = new b3ConvexUtility();
|
||||
bool merge = true;
|
||||
if (numVertices)
|
||||
{
|
||||
utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
|
||||
}
|
||||
|
||||
int collidableIndex = registerConvexHullShape(utilPtr);
|
||||
|
||||
delete utilPtr;
|
||||
return collidableIndex;
|
||||
}
|
||||
|
||||
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
|
||||
{
|
||||
m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
|
||||
m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
|
||||
|
||||
b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
|
||||
convex.mC = convexPtr->mC;
|
||||
convex.mE = convexPtr->mE;
|
||||
convex.m_extents = convexPtr->m_extents;
|
||||
convex.m_localCenter = convexPtr->m_localCenter;
|
||||
convex.m_radius = convexPtr->m_radius;
|
||||
|
||||
convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
|
||||
int edgeOffset = m_data->m_uniqueEdges.size();
|
||||
convex.m_uniqueEdgesOffset = edgeOffset;
|
||||
|
||||
m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
|
||||
|
||||
//convex data here
|
||||
int i;
|
||||
for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
|
||||
{
|
||||
m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
|
||||
}
|
||||
|
||||
int faceOffset = m_data->m_convexFaces.size();
|
||||
convex.m_faceOffset = faceOffset;
|
||||
convex.m_numFaces = convexPtr->m_faces.size();
|
||||
|
||||
m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
|
||||
|
||||
for (i = 0; i < convexPtr->m_faces.size(); i++)
|
||||
{
|
||||
m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
|
||||
convexPtr->m_faces[i].m_plane[1],
|
||||
convexPtr->m_faces[i].m_plane[2],
|
||||
convexPtr->m_faces[i].m_plane[3]);
|
||||
|
||||
int indexOffset = m_data->m_convexIndices.size();
|
||||
int numIndices = convexPtr->m_faces[i].m_indices.size();
|
||||
m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
|
||||
m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
|
||||
m_data->m_convexIndices.resize(indexOffset + numIndices);
|
||||
for (int p = 0; p < numIndices; p++)
|
||||
{
|
||||
m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
|
||||
}
|
||||
}
|
||||
|
||||
convex.m_numVertices = convexPtr->m_vertices.size();
|
||||
int vertexOffset = m_data->m_convexVertices.size();
|
||||
convex.m_vertexOffset = vertexOffset;
|
||||
|
||||
m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
|
||||
for (int i = 0; i < convexPtr->m_vertices.size(); i++)
|
||||
{
|
||||
m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
|
||||
}
|
||||
|
||||
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
|
||||
|
||||
return m_data->m_numAcceleratedShapes++;
|
||||
}
|
||||
|
||||
const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
|
||||
{
|
||||
return m_data->m_localShapeAABBCPU[collidableIndex];
|
||||
}
|
||||
|
|
@ -1,92 +0,0 @@
|
|||
#ifndef B3_CPU_NARROWPHASE_H
|
||||
#define B3_CPU_NARROWPHASE_H
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
class b3CpuNarrowPhase
|
||||
{
|
||||
protected:
|
||||
struct b3CpuNarrowPhaseInternalData* m_data;
|
||||
int m_acceleratedCompanionShapeIndex;
|
||||
int m_planeBodyIndex;
|
||||
int m_static0Index;
|
||||
|
||||
int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
|
||||
int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
|
||||
|
||||
public:
|
||||
b3CpuNarrowPhase(const struct b3Config& config);
|
||||
|
||||
virtual ~b3CpuNarrowPhase(void);
|
||||
|
||||
int registerSphereShape(float radius);
|
||||
int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
|
||||
|
||||
int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
|
||||
int registerFace(const b3Vector3& faceNormal, float faceConstant);
|
||||
|
||||
int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
|
||||
|
||||
//do they need to be merged?
|
||||
|
||||
int registerConvexHullShape(b3ConvexUtility* utilPtr);
|
||||
int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
|
||||
|
||||
//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
|
||||
void setObjectTransform(const float* position, const float* orientation, int bodyIndex);
|
||||
|
||||
void writeAllBodiesToGpu();
|
||||
void reset();
|
||||
void readbackAllBodiesToCpu();
|
||||
bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
|
||||
|
||||
void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
|
||||
void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
|
||||
|
||||
//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
|
||||
virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
|
||||
|
||||
const struct b3RigidBodyData* getBodiesCpu() const;
|
||||
//struct b3RigidBodyData* getBodiesCpu();
|
||||
|
||||
int getNumBodiesGpu() const;
|
||||
|
||||
int getNumBodyInertiasGpu() const;
|
||||
|
||||
const struct b3Collidable* getCollidablesCpu() const;
|
||||
int getNumCollidablesGpu() const;
|
||||
|
||||
/*const struct b3Contact4* getContactsCPU() const;
|
||||
|
||||
|
||||
int getNumContactsGpu() const;
|
||||
*/
|
||||
|
||||
const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
|
||||
|
||||
int getNumRigidBodies() const;
|
||||
|
||||
int allocateCollidable();
|
||||
|
||||
int getStatic0Index() const
|
||||
{
|
||||
return m_static0Index;
|
||||
}
|
||||
b3Collidable& getCollidableCpu(int collidableIndex);
|
||||
const b3Collidable& getCollidableCpu(int collidableIndex) const;
|
||||
|
||||
const b3CpuNarrowPhaseInternalData* getInternalData() const
|
||||
{
|
||||
return m_data;
|
||||
}
|
||||
|
||||
const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
|
||||
};
|
||||
|
||||
#endif //B3_CPU_NARROWPHASE_H
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
|
||||
#ifndef B3_RAYCAST_INFO_H
|
||||
#define B3_RAYCAST_INFO_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3RayInfo
|
||||
{
|
||||
b3Vector3 m_from;
|
||||
b3Vector3 m_to;
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3RayHit
|
||||
{
|
||||
b3Scalar m_hitFraction;
|
||||
int m_hitBody;
|
||||
int m_hitResult1;
|
||||
int m_hitResult2;
|
||||
b3Vector3 m_hitPoint;
|
||||
b3Vector3 m_hitNormal;
|
||||
};
|
||||
|
||||
#endif //B3_RAYCAST_INFO_H
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_RIGID_BODY_CL
|
||||
#define B3_RIGID_BODY_CL
|
||||
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
inline float b3GetInvMass(const b3RigidBodyData& body)
|
||||
{
|
||||
return body.m_invMass;
|
||||
}
|
||||
|
||||
#endif //B3_RIGID_BODY_CL
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
|
||||
#ifndef B3_BVH_SUBTREE_INFO_DATA_H
|
||||
#define B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
|
||||
|
||||
struct b3BvhSubtreeInfoData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes, points to the root of the subtree
|
||||
int m_rootNodeIndex;
|
||||
//4 bytes
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
};
|
||||
|
||||
#endif //B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
|
@ -1,123 +0,0 @@
|
|||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
|
||||
// work-in-progress
|
||||
void b3BvhTraversal(__global const b3Int4* pairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Int4* concavePairsOut,
|
||||
__global volatile int* numConcavePairsOut,
|
||||
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
|
||||
__global const b3QuantizedBvhNode* quantizedNodesRoot,
|
||||
__global const b3BvhInfo* bvhInfos,
|
||||
int numPairs,
|
||||
int maxNumConcavePairsCapacity,
|
||||
int id)
|
||||
{
|
||||
int bodyIndexA = pairs[id].x;
|
||||
int bodyIndexB = pairs[id].y;
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
//once the broadphase avoids static-static pairs, we can remove this test
|
||||
if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
|
||||
return;
|
||||
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
|
||||
if (shapeTypeB != SHAPE_CONVEX_HULL &&
|
||||
shapeTypeB != SHAPE_SPHERE &&
|
||||
shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
return;
|
||||
|
||||
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
|
||||
|
||||
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
|
||||
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
|
||||
b3Float4 bvhQuantization = bvhInfo.m_quantization;
|
||||
int numSubtreeHeaders = bvhInfo.m_numSubTrees;
|
||||
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
|
||||
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
|
||||
|
||||
unsigned short int quantizedQueryAabbMin[3];
|
||||
unsigned short int quantizedQueryAabbMax[3];
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
|
||||
|
||||
for (int i = 0; i < numSubtreeHeaders; i++)
|
||||
{
|
||||
b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
|
||||
|
||||
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
|
||||
if (overlap != 0)
|
||||
{
|
||||
int startNodeIndex = subtree.m_rootNodeIndex;
|
||||
int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
|
||||
int curIndex = startNodeIndex;
|
||||
int escapeIndex;
|
||||
int isLeafNode;
|
||||
int aabbOverlap;
|
||||
while (curIndex < endNodeIndex)
|
||||
{
|
||||
b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
|
||||
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
|
||||
isLeafNode = b3IsLeaf(&rootNode);
|
||||
if (aabbOverlap)
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
int triangleIndex = b3GetTriangleIndex(&rootNode);
|
||||
if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
||||
int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
|
||||
for (int b = 0; b < numChildrenB; b++)
|
||||
{
|
||||
if ((pairIdx + b) < maxNumConcavePairsCapacity)
|
||||
{
|
||||
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
|
||||
concavePairsOut[pairIdx + b] = newPair;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int pairIdx = b3AtomicInc(numConcavePairsOut);
|
||||
if (pairIdx < maxNumConcavePairsCapacity)
|
||||
{
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
|
||||
concavePairsOut[pairIdx] = newPair;
|
||||
}
|
||||
}
|
||||
}
|
||||
curIndex++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
curIndex++;
|
||||
}
|
||||
else
|
||||
{
|
||||
escapeIndex = b3GetEscapeIndex(&rootNode);
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,171 +0,0 @@
|
|||
#ifndef B3_CLIP_FACES_H
|
||||
#define B3_CLIP_FACES_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
|
||||
{
|
||||
return b3MakeFloat4(a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
}
|
||||
|
||||
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
||||
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
|
||||
{
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
//double-check next test
|
||||
// if (numVertsIn < 2)
|
||||
// return 0;
|
||||
|
||||
b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex = pVtxIn[ve];
|
||||
de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
|
||||
if (ds < 0)
|
||||
{
|
||||
if (de < 0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de < 0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
firstVertex = endVertex;
|
||||
ds = de;
|
||||
}
|
||||
return numVertsOut;
|
||||
}
|
||||
|
||||
__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
__global b3Float4* worldVertsB2,
|
||||
int vertexFaceCapacity,
|
||||
int pairIndex)
|
||||
{
|
||||
// int i = get_global_id(0);
|
||||
//int pairIndex = i;
|
||||
int i = pairIndex;
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
// if (i<numPairs)
|
||||
{
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
// int bodyIndexA = pairs[i].x;
|
||||
// int bodyIndexB = pairs[i].y;
|
||||
|
||||
int numLocalContactsOut = 0;
|
||||
|
||||
int capacityWorldVertsB2 = vertexFaceCapacity;
|
||||
|
||||
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
|
||||
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
|
||||
|
||||
{
|
||||
__global b3Int4* clippingFaces = clippingFacesOut;
|
||||
|
||||
int closestFaceA = clippingFaces[pairIndex].x;
|
||||
// int closestFaceB = clippingFaces[pairIndex].y;
|
||||
int numVertsInA = clippingFaces[pairIndex].z;
|
||||
int numVertsInB = clippingFaces[pairIndex].w;
|
||||
|
||||
int numVertsOut = 0;
|
||||
|
||||
if (closestFaceA >= 0)
|
||||
{
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
|
||||
for (int e0 = 0; e0 < numVertsInA; e0++)
|
||||
{
|
||||
const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
|
||||
const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
|
||||
const b3Float4 WorldEdge0 = aw - bw;
|
||||
b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
|
||||
b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = aw;
|
||||
float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS = planeEqWS1;
|
||||
numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
|
||||
__global b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsInB = numVertsOut;
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
|
||||
float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
|
||||
|
||||
for (int i = 0; i < numVertsInB; i++)
|
||||
{
|
||||
float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
|
||||
if (depth <= minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
/*
|
||||
static float maxDepth = 0.f;
|
||||
if (depth < maxDepth)
|
||||
{
|
||||
maxDepth = depth;
|
||||
if (maxDepth < -10)
|
||||
{
|
||||
printf("error at framecount %d?\n",myframecount);
|
||||
}
|
||||
printf("maxDepth = %f\n", maxDepth);
|
||||
|
||||
}
|
||||
*/
|
||||
if (depth <= maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
|
||||
}
|
||||
}
|
||||
}
|
||||
clippingFaces[pairIndex].w = numLocalContactsOut;
|
||||
}
|
||||
|
||||
for (int i = 0; i < numLocalContactsOut; i++)
|
||||
pVtxIn[i] = pVtxOut[i];
|
||||
|
||||
} // if (hasSeparatingAxis[i])
|
||||
} // if (i<numPairs)
|
||||
}
|
||||
|
||||
#endif //B3_CLIP_FACES_H
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
|
||||
#ifndef B3_COLLIDABLE_H
|
||||
#define B3_COLLIDABLE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
enum b3ShapeTypes
|
||||
{
|
||||
SHAPE_HEIGHT_FIELD = 1,
|
||||
|
||||
SHAPE_CONVEX_HULL = 3,
|
||||
SHAPE_PLANE = 4,
|
||||
SHAPE_CONCAVE_TRIMESH = 5,
|
||||
SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
|
||||
SHAPE_SPHERE = 7,
|
||||
MAX_NUM_SHAPE_TYPES,
|
||||
};
|
||||
|
||||
typedef struct b3Collidable b3Collidable_t;
|
||||
|
||||
struct b3Collidable
|
||||
{
|
||||
union {
|
||||
int m_numChildShapes;
|
||||
int m_bvhIndex;
|
||||
};
|
||||
union {
|
||||
float m_radius;
|
||||
int m_compoundBvhIndex;
|
||||
};
|
||||
|
||||
int m_shapeType;
|
||||
union {
|
||||
int m_shapeIndex;
|
||||
float m_height;
|
||||
};
|
||||
};
|
||||
|
||||
typedef struct b3GpuChildShape b3GpuChildShape_t;
|
||||
struct b3GpuChildShape
|
||||
{
|
||||
b3Float4 m_childPosition;
|
||||
b3Quat m_childOrientation;
|
||||
union {
|
||||
int m_shapeIndex; //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
int m_capsuleAxis;
|
||||
};
|
||||
union {
|
||||
float m_radius; //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_numChildShapes; //used for compound shape
|
||||
};
|
||||
union {
|
||||
float m_height; //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
|
||||
int m_collidableShapeIndex;
|
||||
};
|
||||
int m_shapeType;
|
||||
};
|
||||
|
||||
struct b3CompoundOverlappingPair
|
||||
{
|
||||
int m_bodyIndexA;
|
||||
int m_bodyIndexB;
|
||||
// int m_pairType;
|
||||
int m_childShapeIndexA;
|
||||
int m_childShapeIndexB;
|
||||
};
|
||||
|
||||
#endif //B3_COLLIDABLE_H
|
||||
|
|
@ -1,36 +0,0 @@
|
|||
#ifndef B3_CONTACT4DATA_H
|
||||
#define B3_CONTACT4DATA_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
typedef struct b3Contact4Data b3Contact4Data_t;
|
||||
|
||||
struct b3Contact4Data
|
||||
{
|
||||
b3Float4 m_worldPosB[4];
|
||||
// b3Float4 m_localPosA[4];
|
||||
// b3Float4 m_localPosB[4];
|
||||
b3Float4 m_worldNormalOnB; // w: m_nPoints
|
||||
unsigned short m_restituitionCoeffCmp;
|
||||
unsigned short m_frictionCoeffCmp;
|
||||
int m_batchIdx;
|
||||
int m_bodyAPtrAndSignBit; //x:m_bodyAPtr, y:m_bodyBPtr
|
||||
int m_bodyBPtrAndSignBit;
|
||||
|
||||
int m_childIndexA;
|
||||
int m_childIndexB;
|
||||
int m_unused1;
|
||||
int m_unused2;
|
||||
};
|
||||
|
||||
inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
|
||||
{
|
||||
return (int)contact->m_worldNormalOnB.w;
|
||||
};
|
||||
|
||||
inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
|
||||
{
|
||||
contact->m_worldNormalOnB.w = (float)numPoints;
|
||||
};
|
||||
|
||||
#endif //B3_CONTACT4DATA_H
|
||||
|
|
@ -1,486 +0,0 @@
|
|||
|
||||
#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
#define B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
|
||||
|
||||
#define B3_MAX_VERTS 1024
|
||||
|
||||
inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
|
||||
{
|
||||
return b3MakeVector3(a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
0.f);
|
||||
}
|
||||
|
||||
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
|
||||
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
|
||||
{
|
||||
int ve;
|
||||
float ds, de;
|
||||
int numVertsOut = 0;
|
||||
if (numVertsIn < 2)
|
||||
return 0;
|
||||
|
||||
b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
|
||||
b3Float4 endVertex = pVtxIn[0];
|
||||
|
||||
ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVertsIn; ve++)
|
||||
{
|
||||
endVertex = pVtxIn[ve];
|
||||
|
||||
de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
|
||||
|
||||
if (ds < 0)
|
||||
{
|
||||
if (de < 0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de < 0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
|
||||
ppVtxOut[numVertsOut++] = endVertex;
|
||||
}
|
||||
}
|
||||
firstVertex = endVertex;
|
||||
ds = de;
|
||||
}
|
||||
return numVertsOut;
|
||||
}
|
||||
|
||||
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
|
||||
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
|
||||
b3Float4* worldVertsB2, int capacityWorldVertsB2,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
//const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
|
||||
b3Float4* pVtxIn = worldVertsB1;
|
||||
b3Float4* pVtxOut = worldVertsB2;
|
||||
|
||||
int numVertsIn = numWorldVertsB1;
|
||||
int numVertsOut = 0;
|
||||
|
||||
int closestFaceA = -1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for (int face = 0; face < hullA->m_numFaces; face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeVector3(
|
||||
facesA[hullA->m_faceOffset + face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset + face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
|
||||
|
||||
float d = b3Dot3F4(faceANormalWS, separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (closestFaceA < 0)
|
||||
return numContactsOut;
|
||||
|
||||
b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
|
||||
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
//int numContacts = numWorldVertsB1;
|
||||
int numVerticesA = polyA.m_numIndices;
|
||||
for (int e0 = 0; e0 < numVerticesA; e0++)
|
||||
{
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
|
||||
const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
|
||||
const b3Float4 edge0 = a - b;
|
||||
const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
|
||||
b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
|
||||
b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
|
||||
|
||||
b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
|
||||
b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
|
||||
float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
|
||||
|
||||
b3Float4 planeNormalWS = planeNormalWS1;
|
||||
float planeEqWS = planeEqWS1;
|
||||
|
||||
//clip face
|
||||
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
|
||||
numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
|
||||
|
||||
//btSwap(pVtxIn,pVtxOut);
|
||||
b3Float4* tmp = pVtxOut;
|
||||
pVtxOut = pVtxIn;
|
||||
pVtxIn = tmp;
|
||||
numVertsIn = numVertsOut;
|
||||
numVertsOut = 0;
|
||||
}
|
||||
|
||||
// only keep points that are behind the witness face
|
||||
{
|
||||
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
|
||||
float localPlaneEq = polyA.m_plane.w;
|
||||
b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
|
||||
float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
|
||||
for (int i = 0; i < numVertsIn; i++)
|
||||
{
|
||||
float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
|
||||
if (depth <= minDist)
|
||||
{
|
||||
depth = minDist;
|
||||
}
|
||||
if (numContactsOut < contactCapacity)
|
||||
{
|
||||
if (depth <= maxDist)
|
||||
{
|
||||
b3Float4 pointInWorld = pVtxIn[i];
|
||||
//resultOut.addContactPoint(separatingNormal,point,depth);
|
||||
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
|
||||
//printf("depth=%f\n",depth);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
|
||||
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
|
||||
b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
b3Float4* contactsOut,
|
||||
int contactCapacity)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1 = 0;
|
||||
|
||||
B3_PROFILE("clipHullAgainstHull");
|
||||
|
||||
//float curMaxDist=maxDist;
|
||||
int closestFaceB = -1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
//B3_PROFILE("closestFaceB");
|
||||
if (hullB.m_numFaces != 1)
|
||||
{
|
||||
//printf("wtf\n");
|
||||
}
|
||||
static bool once = true;
|
||||
//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
|
||||
|
||||
for (int face = 0; face < hullB.m_numFaces; face++)
|
||||
{
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("face %d\n", face);
|
||||
const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
|
||||
if (once)
|
||||
{
|
||||
for (int i = 0; i < faceB->m_numIndices; i++)
|
||||
{
|
||||
b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
|
||||
printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
|
||||
}
|
||||
}
|
||||
#endif //BT_DEBUG_SAT_FACE \
|
||||
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
|
||||
facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
|
||||
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
#ifdef BT_DEBUG_SAT_FACE
|
||||
if (once)
|
||||
printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
|
||||
#endif
|
||||
float d = b3Dot3F4(WorldNormal, separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
once = false;
|
||||
}
|
||||
|
||||
b3Assert(closestFaceB >= 0);
|
||||
{
|
||||
//B3_PROFILE("worldVertsB1");
|
||||
const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for (int e0 = 0; e0 < numVertices; e0++)
|
||||
{
|
||||
const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
|
||||
worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
|
||||
}
|
||||
}
|
||||
|
||||
if (closestFaceB >= 0)
|
||||
{
|
||||
//B3_PROFILE("clipFaceAgainstHull");
|
||||
numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
|
||||
posA, ornA,
|
||||
worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
|
||||
verticesA, facesA, indicesA,
|
||||
contactsOut, contactCapacity);
|
||||
}
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
inline int b3ClipHullHullSingle(
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
const b3Float4& posA,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB,
|
||||
const b3Quaternion& ornB,
|
||||
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
|
||||
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
|
||||
b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
|
||||
int& nContacts,
|
||||
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
|
||||
const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
|
||||
const b3Vector3& sepNormalWorldSpace,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
int contactIndex = -1;
|
||||
b3ConvexPolyhedronData hullA, hullB;
|
||||
|
||||
b3Collidable colA = hostCollidablesA[collidableIndexA];
|
||||
hullA = hostConvexDataA[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
b3Collidable colB = hostCollidablesB[collidableIndexB];
|
||||
hullB = hostConvexDataB[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
b3Float4 contactsOut[B3_MAX_VERTS];
|
||||
int localContactCapacity = B3_MAX_VERTS;
|
||||
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
|
||||
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
|
||||
#endif
|
||||
|
||||
{
|
||||
b3Float4 worldVertsB1[B3_MAX_VERTS];
|
||||
b3Float4 worldVertsB2[B3_MAX_VERTS];
|
||||
int capacityWorldVerts = B3_MAX_VERTS;
|
||||
|
||||
b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
|
||||
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
|
||||
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
|
||||
|
||||
b3Scalar minDist = -1;
|
||||
b3Scalar maxDist = 0.;
|
||||
|
||||
b3Transform trA, trB;
|
||||
{
|
||||
//B3_PROFILE("b3TransformPoint computation");
|
||||
//trA.setIdentity();
|
||||
trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
|
||||
trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
|
||||
|
||||
//trB.setIdentity();
|
||||
trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
|
||||
trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
|
||||
}
|
||||
|
||||
b3Quaternion trAorn = trA.getRotation();
|
||||
b3Quaternion trBorn = trB.getRotation();
|
||||
|
||||
int numContactsOut = b3ClipHullAgainstHull(hostNormal,
|
||||
hostConvexDataA.at(shapeA),
|
||||
hostConvexDataB.at(shapeB),
|
||||
(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
|
||||
(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
|
||||
worldVertsB1, worldVertsB2, capacityWorldVerts,
|
||||
minDist, maxDist,
|
||||
verticesA, facesA, indicesA,
|
||||
verticesB, facesB, indicesB,
|
||||
|
||||
contactsOut, localContactCapacity);
|
||||
|
||||
if (numContactsOut > 0)
|
||||
{
|
||||
B3_PROFILE("overlap");
|
||||
|
||||
b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
|
||||
// b3Float4 centerOut;
|
||||
|
||||
b3Int4 contactIdx;
|
||||
contactIdx.x = 0;
|
||||
contactIdx.y = 1;
|
||||
contactIdx.z = 2;
|
||||
contactIdx.w = 3;
|
||||
|
||||
int numPoints = 0;
|
||||
|
||||
{
|
||||
B3_PROFILE("extractManifold");
|
||||
numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
|
||||
}
|
||||
|
||||
b3Assert(numPoints);
|
||||
|
||||
if (nContacts < maxContactCapacity)
|
||||
{
|
||||
contactIndex = nContacts;
|
||||
globalContactOut->expand();
|
||||
b3Contact4Data& contact = globalContactOut->at(nContacts);
|
||||
contact.m_batchIdx = 0; //i;
|
||||
contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
|
||||
contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
|
||||
|
||||
contact.m_frictionCoeffCmp = 45874;
|
||||
contact.m_restituitionCoeffCmp = 0;
|
||||
|
||||
// float distance = 0.f;
|
||||
for (int p = 0; p < numPoints; p++)
|
||||
{
|
||||
contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
|
||||
contact.m_worldNormalOnB = normalOnSurfaceB;
|
||||
}
|
||||
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
|
||||
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
nContacts++;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
|
||||
}
|
||||
}
|
||||
}
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
inline int b3ContactConvexConvexSAT(
|
||||
int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
|
||||
const b3AlignedObjectArray<b3Collidable>& collidables,
|
||||
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
|
||||
const b3AlignedObjectArray<b3Float4>& convexVertices,
|
||||
const b3AlignedObjectArray<b3Float4>& uniqueEdges,
|
||||
const b3AlignedObjectArray<int>& convexIndices,
|
||||
const b3AlignedObjectArray<b3GpuFace>& faces,
|
||||
b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
int contactIndex = -1;
|
||||
|
||||
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
b3ConvexPolyhedronData hullA, hullB;
|
||||
|
||||
b3Float4 sepNormalWorldSpace;
|
||||
|
||||
b3Collidable colA = collidables[collidableIndexA];
|
||||
hullA = convexShapes[colA.m_shapeIndex];
|
||||
//printf("numvertsA = %d\n",hullA.m_numVertices);
|
||||
|
||||
b3Collidable colB = collidables[collidableIndexB];
|
||||
hullB = convexShapes[colB.m_shapeIndex];
|
||||
//printf("numvertsB = %d\n",hullB.m_numVertices);
|
||||
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
|
||||
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
|
||||
#endif
|
||||
|
||||
bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
|
||||
posA,
|
||||
ornA,
|
||||
posB,
|
||||
ornB,
|
||||
|
||||
convexVertices, uniqueEdges, faces, convexIndices,
|
||||
convexVertices, uniqueEdges, faces, convexIndices,
|
||||
|
||||
sepNormalWorldSpace);
|
||||
|
||||
if (foundSepAxis)
|
||||
{
|
||||
contactIndex = b3ClipHullHullSingle(
|
||||
bodyIndexA, bodyIndexB,
|
||||
posA, ornA,
|
||||
posB, ornB,
|
||||
collidableIndexA, collidableIndexB,
|
||||
&rigidBodies,
|
||||
&globalContactsOut,
|
||||
nGlobalContactsOut,
|
||||
|
||||
convexShapes,
|
||||
convexShapes,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
convexVertices,
|
||||
uniqueEdges,
|
||||
faces,
|
||||
convexIndices,
|
||||
|
||||
collidables,
|
||||
collidables,
|
||||
sepNormalWorldSpace,
|
||||
maxContactCapacity);
|
||||
}
|
||||
|
||||
return contactIndex;
|
||||
}
|
||||
|
||||
#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
|
||||
|
|
@ -1,153 +0,0 @@
|
|||
|
||||
#ifndef B3_CONTACT_SPHERE_SPHERE_H
|
||||
#define B3_CONTACT_SPHERE_SPHERE_H
|
||||
|
||||
void computeContactSphereConvex(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const b3RigidBodyData* rigidBodies,
|
||||
const b3Collidable* collidables,
|
||||
const b3ConvexPolyhedronData* convexShapes,
|
||||
const b3Vector3* convexVertices,
|
||||
const int* convexIndices,
|
||||
const b3GpuFace* faces,
|
||||
b3Contact4* globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
float radius = collidables[collidableIndexA].m_radius;
|
||||
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
|
||||
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
|
||||
|
||||
float4 pos = rigidBodies[bodyIndexB].m_pos;
|
||||
|
||||
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
b3Transform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(pos);
|
||||
tr.setRotation(quat);
|
||||
b3Transform trInv = tr.inverse();
|
||||
|
||||
float4 spherePos = trInv(spherePos1);
|
||||
|
||||
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
int numFaces = convexShapes[shapeIndex].m_numFaces;
|
||||
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
|
||||
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
|
||||
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
|
||||
bool bCollide = true;
|
||||
int region = -1;
|
||||
float4 localHitNormal;
|
||||
for (int f = 0; f < numFaces; f++)
|
||||
{
|
||||
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
|
||||
float4 planeEqn;
|
||||
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
|
||||
float4 n1 = localPlaneNormal; //quatRotate(quat,localPlaneNormal);
|
||||
planeEqn = n1;
|
||||
planeEqn[3] = face.m_plane.w;
|
||||
|
||||
float4 pntReturn;
|
||||
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
|
||||
|
||||
if (dist > radius)
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (dist > 0)
|
||||
{
|
||||
//might hit an edge or vertex
|
||||
b3Vector3 out;
|
||||
|
||||
bool isInPoly = IsPointInPolygon(spherePos,
|
||||
&face,
|
||||
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
|
||||
convexIndices,
|
||||
&out);
|
||||
if (isInPoly)
|
||||
{
|
||||
if (dist > minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Vector3 tmp = spherePos - out;
|
||||
b3Scalar l2 = tmp.length2();
|
||||
if (l2 < radius * radius)
|
||||
{
|
||||
dist = b3Sqrt(l2);
|
||||
if (dist > minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = out;
|
||||
localHitNormal = tmp / dist;
|
||||
region = 2;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bCollide = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (dist > minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
closestPnt = pntReturn;
|
||||
localHitNormal = planeEqn;
|
||||
region = 3;
|
||||
}
|
||||
}
|
||||
}
|
||||
static int numChecks = 0;
|
||||
numChecks++;
|
||||
|
||||
if (bCollide && minDist > -10000)
|
||||
{
|
||||
float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
|
||||
float4 pOnB1 = tr(closestPnt);
|
||||
//printf("dist ,%f,",minDist);
|
||||
float actualDepth = minDist - radius;
|
||||
if (actualDepth < 0)
|
||||
{
|
||||
//printf("actualDepth = ,%f,", actualDepth);
|
||||
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
|
||||
//printf("region=,%d,\n", region);
|
||||
pOnB1[3] = actualDepth;
|
||||
|
||||
int dstIdx;
|
||||
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
||||
|
||||
if (nGlobalContactsOut < maxContactCapacity)
|
||||
{
|
||||
dstIdx = nGlobalContactsOut;
|
||||
nGlobalContactsOut++;
|
||||
|
||||
b3Contact4* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = normalOnSurfaceB1;
|
||||
c->setFrictionCoeff(0.7);
|
||||
c->setRestituitionCoeff(0.f);
|
||||
|
||||
c->m_batchIdx = pairIndex;
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
|
||||
c->m_worldPosB[0] = pOnB1;
|
||||
int numPoints = 1;
|
||||
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
|
||||
} //if (dstIdx < numPairs)
|
||||
}
|
||||
} //if (hasCollision)
|
||||
}
|
||||
#endif //B3_CONTACT_SPHERE_SPHERE_H
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
|
||||
#ifndef B3_CONVEX_POLYHEDRON_DATA_H
|
||||
#define B3_CONVEX_POLYHEDRON_DATA_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
typedef struct b3GpuFace b3GpuFace_t;
|
||||
struct b3GpuFace
|
||||
{
|
||||
b3Float4 m_plane;
|
||||
int m_indexOffset;
|
||||
int m_numIndices;
|
||||
int m_unusedPadding1;
|
||||
int m_unusedPadding2;
|
||||
};
|
||||
|
||||
typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
|
||||
|
||||
struct b3ConvexPolyhedronData
|
||||
{
|
||||
b3Float4 m_localCenter;
|
||||
b3Float4 m_extents;
|
||||
b3Float4 mC;
|
||||
b3Float4 mE;
|
||||
|
||||
float m_radius;
|
||||
int m_faceOffset;
|
||||
int m_numFaces;
|
||||
int m_numVertices;
|
||||
|
||||
int m_vertexOffset;
|
||||
int m_uniqueEdgesOffset;
|
||||
int m_numUniqueEdges;
|
||||
int m_unused;
|
||||
};
|
||||
|
||||
#endif //B3_CONVEX_POLYHEDRON_DATA_H
|
||||
|
|
@ -1,797 +0,0 @@
|
|||
#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
|
||||
#define B3_TRIANGLE_NUM_CONVEX_FACES 5
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
|
||||
const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
|
||||
{
|
||||
min[0] = FLT_MAX;
|
||||
max[0] = -FLT_MAX;
|
||||
int numVerts = hull->m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
|
||||
float offset = b3Dot(pos, *dir);
|
||||
for (int i = 0; i < numVerts; i++)
|
||||
{
|
||||
float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
|
||||
if (dp < min[0])
|
||||
min[0] = dp;
|
||||
if (dp > max[0])
|
||||
max[0] = dp;
|
||||
}
|
||||
if (min[0] > max[0])
|
||||
{
|
||||
float tmp = min[0];
|
||||
min[0] = max[0];
|
||||
max[0] = tmp;
|
||||
}
|
||||
min[0] += offset;
|
||||
max[0] += offset;
|
||||
}
|
||||
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA, b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB, b3QuatConstArg ornB,
|
||||
b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
|
||||
{
|
||||
float Min0, Max0;
|
||||
float Min1, Max1;
|
||||
b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
|
||||
b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
|
||||
|
||||
if (Max0 < Min1 || Max1 < Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
*depth = d0 < d1 ? d0 : d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA1,
|
||||
b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB1,
|
||||
b3QuatConstArg ornB,
|
||||
b3Float4ConstArg DeltaC2,
|
||||
|
||||
const b3Float4* verticesA,
|
||||
const b3Float4* uniqueEdgesA,
|
||||
const b3GpuFace* facesA,
|
||||
const int* indicesA,
|
||||
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3Float4* uniqueEdgesB,
|
||||
__global const b3GpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
b3Float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
/*
|
||||
static int maxFaceVertex = 0;
|
||||
|
||||
int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
|
||||
curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
|
||||
|
||||
if (curFaceVertexAB>maxFaceVertex)
|
||||
{
|
||||
maxFaceVertex = curFaceVertexAB;
|
||||
printf("curFaceVertexAB = %d\n",curFaceVertexAB);
|
||||
printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
|
||||
printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
|
||||
printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
|
||||
}
|
||||
*/
|
||||
|
||||
int curPlaneTests = 0;
|
||||
{
|
||||
int numFacesA = hullA->m_numFaces;
|
||||
// Test normals from hullA
|
||||
for (int i = 0; i < numFacesA; i++)
|
||||
{
|
||||
const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
|
||||
if (b3Dot(DeltaC2, faceANormalWS) < 0)
|
||||
faceANormalWS *= -1.f;
|
||||
curPlaneTests++;
|
||||
float d;
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
|
||||
return false;
|
||||
if (d < *dmin)
|
||||
{
|
||||
*dmin = d;
|
||||
*sep = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
b3Vector3 unitSphere162[] =
|
||||
{
|
||||
b3MakeVector3(0.000000, -1.000000, 0.000000),
|
||||
b3MakeVector3(0.203181, -0.967950, 0.147618),
|
||||
b3MakeVector3(-0.077607, -0.967950, 0.238853),
|
||||
b3MakeVector3(0.723607, -0.447220, 0.525725),
|
||||
b3MakeVector3(0.609547, -0.657519, 0.442856),
|
||||
b3MakeVector3(0.812729, -0.502301, 0.295238),
|
||||
b3MakeVector3(-0.251147, -0.967949, 0.000000),
|
||||
b3MakeVector3(-0.077607, -0.967950, -0.238853),
|
||||
b3MakeVector3(0.203181, -0.967950, -0.147618),
|
||||
b3MakeVector3(0.860698, -0.251151, 0.442858),
|
||||
b3MakeVector3(-0.276388, -0.447220, 0.850649),
|
||||
b3MakeVector3(-0.029639, -0.502302, 0.864184),
|
||||
b3MakeVector3(-0.155215, -0.251152, 0.955422),
|
||||
b3MakeVector3(-0.894426, -0.447216, 0.000000),
|
||||
b3MakeVector3(-0.831051, -0.502299, 0.238853),
|
||||
b3MakeVector3(-0.956626, -0.251149, 0.147618),
|
||||
b3MakeVector3(-0.276388, -0.447220, -0.850649),
|
||||
b3MakeVector3(-0.483971, -0.502302, -0.716565),
|
||||
b3MakeVector3(-0.436007, -0.251152, -0.864188),
|
||||
b3MakeVector3(0.723607, -0.447220, -0.525725),
|
||||
b3MakeVector3(0.531941, -0.502302, -0.681712),
|
||||
b3MakeVector3(0.687159, -0.251152, -0.681715),
|
||||
b3MakeVector3(0.687159, -0.251152, 0.681715),
|
||||
b3MakeVector3(-0.436007, -0.251152, 0.864188),
|
||||
b3MakeVector3(-0.956626, -0.251149, -0.147618),
|
||||
b3MakeVector3(-0.155215, -0.251152, -0.955422),
|
||||
b3MakeVector3(0.860698, -0.251151, -0.442858),
|
||||
b3MakeVector3(0.276388, 0.447220, 0.850649),
|
||||
b3MakeVector3(0.483971, 0.502302, 0.716565),
|
||||
b3MakeVector3(0.232822, 0.657519, 0.716563),
|
||||
b3MakeVector3(-0.723607, 0.447220, 0.525725),
|
||||
b3MakeVector3(-0.531941, 0.502302, 0.681712),
|
||||
b3MakeVector3(-0.609547, 0.657519, 0.442856),
|
||||
b3MakeVector3(-0.723607, 0.447220, -0.525725),
|
||||
b3MakeVector3(-0.812729, 0.502301, -0.295238),
|
||||
b3MakeVector3(-0.609547, 0.657519, -0.442856),
|
||||
b3MakeVector3(0.276388, 0.447220, -0.850649),
|
||||
b3MakeVector3(0.029639, 0.502302, -0.864184),
|
||||
b3MakeVector3(0.232822, 0.657519, -0.716563),
|
||||
b3MakeVector3(0.894426, 0.447216, 0.000000),
|
||||
b3MakeVector3(0.831051, 0.502299, -0.238853),
|
||||
b3MakeVector3(0.753442, 0.657515, 0.000000),
|
||||
b3MakeVector3(-0.232822, -0.657519, 0.716563),
|
||||
b3MakeVector3(-0.162456, -0.850654, 0.499995),
|
||||
b3MakeVector3(0.052790, -0.723612, 0.688185),
|
||||
b3MakeVector3(0.138199, -0.894429, 0.425321),
|
||||
b3MakeVector3(0.262869, -0.525738, 0.809012),
|
||||
b3MakeVector3(0.361805, -0.723611, 0.587779),
|
||||
b3MakeVector3(0.531941, -0.502302, 0.681712),
|
||||
b3MakeVector3(0.425323, -0.850654, 0.309011),
|
||||
b3MakeVector3(0.812729, -0.502301, -0.295238),
|
||||
b3MakeVector3(0.609547, -0.657519, -0.442856),
|
||||
b3MakeVector3(0.850648, -0.525736, 0.000000),
|
||||
b3MakeVector3(0.670817, -0.723611, -0.162457),
|
||||
b3MakeVector3(0.670817, -0.723610, 0.162458),
|
||||
b3MakeVector3(0.425323, -0.850654, -0.309011),
|
||||
b3MakeVector3(0.447211, -0.894428, 0.000001),
|
||||
b3MakeVector3(-0.753442, -0.657515, 0.000000),
|
||||
b3MakeVector3(-0.525730, -0.850652, 0.000000),
|
||||
b3MakeVector3(-0.638195, -0.723609, 0.262864),
|
||||
b3MakeVector3(-0.361801, -0.894428, 0.262864),
|
||||
b3MakeVector3(-0.688189, -0.525736, 0.499997),
|
||||
b3MakeVector3(-0.447211, -0.723610, 0.525729),
|
||||
b3MakeVector3(-0.483971, -0.502302, 0.716565),
|
||||
b3MakeVector3(-0.232822, -0.657519, -0.716563),
|
||||
b3MakeVector3(-0.162456, -0.850654, -0.499995),
|
||||
b3MakeVector3(-0.447211, -0.723611, -0.525727),
|
||||
b3MakeVector3(-0.361801, -0.894429, -0.262863),
|
||||
b3MakeVector3(-0.688189, -0.525736, -0.499997),
|
||||
b3MakeVector3(-0.638195, -0.723609, -0.262863),
|
||||
b3MakeVector3(-0.831051, -0.502299, -0.238853),
|
||||
b3MakeVector3(0.361804, -0.723612, -0.587779),
|
||||
b3MakeVector3(0.138197, -0.894429, -0.425321),
|
||||
b3MakeVector3(0.262869, -0.525738, -0.809012),
|
||||
b3MakeVector3(0.052789, -0.723611, -0.688186),
|
||||
b3MakeVector3(-0.029639, -0.502302, -0.864184),
|
||||
b3MakeVector3(0.956626, 0.251149, 0.147618),
|
||||
b3MakeVector3(0.956626, 0.251149, -0.147618),
|
||||
b3MakeVector3(0.951058, -0.000000, 0.309013),
|
||||
b3MakeVector3(1.000000, 0.000000, 0.000000),
|
||||
b3MakeVector3(0.947213, -0.276396, 0.162458),
|
||||
b3MakeVector3(0.951058, 0.000000, -0.309013),
|
||||
b3MakeVector3(0.947213, -0.276396, -0.162458),
|
||||
b3MakeVector3(0.155215, 0.251152, 0.955422),
|
||||
b3MakeVector3(0.436007, 0.251152, 0.864188),
|
||||
b3MakeVector3(-0.000000, -0.000000, 1.000000),
|
||||
b3MakeVector3(0.309017, 0.000000, 0.951056),
|
||||
b3MakeVector3(0.138199, -0.276398, 0.951055),
|
||||
b3MakeVector3(0.587786, 0.000000, 0.809017),
|
||||
b3MakeVector3(0.447216, -0.276398, 0.850648),
|
||||
b3MakeVector3(-0.860698, 0.251151, 0.442858),
|
||||
b3MakeVector3(-0.687159, 0.251152, 0.681715),
|
||||
b3MakeVector3(-0.951058, -0.000000, 0.309013),
|
||||
b3MakeVector3(-0.809018, 0.000000, 0.587783),
|
||||
b3MakeVector3(-0.861803, -0.276396, 0.425324),
|
||||
b3MakeVector3(-0.587786, 0.000000, 0.809017),
|
||||
b3MakeVector3(-0.670819, -0.276397, 0.688191),
|
||||
b3MakeVector3(-0.687159, 0.251152, -0.681715),
|
||||
b3MakeVector3(-0.860698, 0.251151, -0.442858),
|
||||
b3MakeVector3(-0.587786, -0.000000, -0.809017),
|
||||
b3MakeVector3(-0.809018, -0.000000, -0.587783),
|
||||
b3MakeVector3(-0.670819, -0.276397, -0.688191),
|
||||
b3MakeVector3(-0.951058, 0.000000, -0.309013),
|
||||
b3MakeVector3(-0.861803, -0.276396, -0.425324),
|
||||
b3MakeVector3(0.436007, 0.251152, -0.864188),
|
||||
b3MakeVector3(0.155215, 0.251152, -0.955422),
|
||||
b3MakeVector3(0.587786, -0.000000, -0.809017),
|
||||
b3MakeVector3(0.309017, -0.000000, -0.951056),
|
||||
b3MakeVector3(0.447216, -0.276398, -0.850648),
|
||||
b3MakeVector3(0.000000, 0.000000, -1.000000),
|
||||
b3MakeVector3(0.138199, -0.276398, -0.951055),
|
||||
b3MakeVector3(0.670820, 0.276396, 0.688190),
|
||||
b3MakeVector3(0.809019, -0.000002, 0.587783),
|
||||
b3MakeVector3(0.688189, 0.525736, 0.499997),
|
||||
b3MakeVector3(0.861804, 0.276394, 0.425323),
|
||||
b3MakeVector3(0.831051, 0.502299, 0.238853),
|
||||
b3MakeVector3(-0.447216, 0.276397, 0.850649),
|
||||
b3MakeVector3(-0.309017, -0.000001, 0.951056),
|
||||
b3MakeVector3(-0.262869, 0.525738, 0.809012),
|
||||
b3MakeVector3(-0.138199, 0.276397, 0.951055),
|
||||
b3MakeVector3(0.029639, 0.502302, 0.864184),
|
||||
b3MakeVector3(-0.947213, 0.276396, -0.162458),
|
||||
b3MakeVector3(-1.000000, 0.000001, 0.000000),
|
||||
b3MakeVector3(-0.850648, 0.525736, -0.000000),
|
||||
b3MakeVector3(-0.947213, 0.276397, 0.162458),
|
||||
b3MakeVector3(-0.812729, 0.502301, 0.295238),
|
||||
b3MakeVector3(-0.138199, 0.276397, -0.951055),
|
||||
b3MakeVector3(-0.309016, -0.000000, -0.951057),
|
||||
b3MakeVector3(-0.262869, 0.525738, -0.809012),
|
||||
b3MakeVector3(-0.447215, 0.276397, -0.850649),
|
||||
b3MakeVector3(-0.531941, 0.502302, -0.681712),
|
||||
b3MakeVector3(0.861804, 0.276396, -0.425322),
|
||||
b3MakeVector3(0.809019, 0.000000, -0.587782),
|
||||
b3MakeVector3(0.688189, 0.525736, -0.499997),
|
||||
b3MakeVector3(0.670821, 0.276397, -0.688189),
|
||||
b3MakeVector3(0.483971, 0.502302, -0.716565),
|
||||
b3MakeVector3(0.077607, 0.967950, 0.238853),
|
||||
b3MakeVector3(0.251147, 0.967949, 0.000000),
|
||||
b3MakeVector3(0.000000, 1.000000, 0.000000),
|
||||
b3MakeVector3(0.162456, 0.850654, 0.499995),
|
||||
b3MakeVector3(0.361800, 0.894429, 0.262863),
|
||||
b3MakeVector3(0.447209, 0.723612, 0.525728),
|
||||
b3MakeVector3(0.525730, 0.850652, 0.000000),
|
||||
b3MakeVector3(0.638194, 0.723610, 0.262864),
|
||||
b3MakeVector3(-0.203181, 0.967950, 0.147618),
|
||||
b3MakeVector3(-0.425323, 0.850654, 0.309011),
|
||||
b3MakeVector3(-0.138197, 0.894430, 0.425320),
|
||||
b3MakeVector3(-0.361804, 0.723612, 0.587778),
|
||||
b3MakeVector3(-0.052790, 0.723612, 0.688185),
|
||||
b3MakeVector3(-0.203181, 0.967950, -0.147618),
|
||||
b3MakeVector3(-0.425323, 0.850654, -0.309011),
|
||||
b3MakeVector3(-0.447210, 0.894429, 0.000000),
|
||||
b3MakeVector3(-0.670817, 0.723611, -0.162457),
|
||||
b3MakeVector3(-0.670817, 0.723611, 0.162457),
|
||||
b3MakeVector3(0.077607, 0.967950, -0.238853),
|
||||
b3MakeVector3(0.162456, 0.850654, -0.499995),
|
||||
b3MakeVector3(-0.138197, 0.894430, -0.425320),
|
||||
b3MakeVector3(-0.052790, 0.723612, -0.688185),
|
||||
b3MakeVector3(-0.361804, 0.723612, -0.587778),
|
||||
b3MakeVector3(0.361800, 0.894429, -0.262863),
|
||||
b3MakeVector3(0.638194, 0.723610, -0.262864),
|
||||
b3MakeVector3(0.447209, 0.723612, -0.525728)};
|
||||
|
||||
bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA1,
|
||||
b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB1,
|
||||
b3QuatConstArg ornB,
|
||||
b3Float4ConstArg DeltaC2,
|
||||
const b3Float4* verticesA,
|
||||
const b3Float4* uniqueEdgesA,
|
||||
const b3GpuFace* facesA,
|
||||
const int* indicesA,
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3Float4* uniqueEdgesB,
|
||||
__global const b3GpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
b3Float4* sep,
|
||||
float* dmin,
|
||||
bool searchAllEdgeEdge)
|
||||
{
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
|
||||
// int curPlaneTests=0;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
static int maxEdgeTests = 0;
|
||||
int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
|
||||
if (curEdgeTests > maxEdgeTests)
|
||||
{
|
||||
maxEdgeTests = curEdgeTests;
|
||||
printf("maxEdgeTests = %d\n", maxEdgeTests);
|
||||
printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
|
||||
printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
|
||||
}
|
||||
|
||||
if (searchAllEdgeEdge)
|
||||
{
|
||||
for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
|
||||
{
|
||||
const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA, edge0);
|
||||
|
||||
for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
|
||||
{
|
||||
const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB, edge1);
|
||||
|
||||
b3Float4 crossje = b3Cross(edge0World, edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if (!b3IsAlmostZero(crossje))
|
||||
{
|
||||
crossje = b3Normalized(crossje);
|
||||
if (b3Dot(DeltaC2, crossje) < 0)
|
||||
crossje *= -1.f;
|
||||
|
||||
float dist;
|
||||
bool result = true;
|
||||
{
|
||||
float Min0, Max0;
|
||||
float Min1, Max1;
|
||||
b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
|
||||
b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
|
||||
|
||||
if (Max0 < Min1 || Max1 < Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0 < d1 ? d0 : d1;
|
||||
result = true;
|
||||
}
|
||||
|
||||
if (dist < *dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
|
||||
//printf("numDirections =%d\n",numDirections );
|
||||
|
||||
for (int i = 0; i < numDirections; i++)
|
||||
{
|
||||
b3Float4 crossje = unitSphere162[i];
|
||||
{
|
||||
//if (b3Dot(DeltaC2,crossje)>0)
|
||||
{
|
||||
float dist;
|
||||
bool result = true;
|
||||
{
|
||||
float Min0, Max0;
|
||||
float Min1, Max1;
|
||||
b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
|
||||
b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
|
||||
|
||||
if (Max0 < Min1 || Max1 < Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0 < d1 ? d0 : d1;
|
||||
result = true;
|
||||
}
|
||||
|
||||
if (dist < *dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
|
||||
__global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
|
||||
b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
__global const b3Float4* verticesA,
|
||||
__global const b3GpuFace_t* facesA,
|
||||
__global const int* indicesA,
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3GpuFace_t* facesB,
|
||||
__global const int* indicesB,
|
||||
|
||||
__global b3Int4* clippingFaces, int pairIndex)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1 = 0;
|
||||
|
||||
int closestFaceB = -1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
for (int face = 0; face < hullB->m_numFaces; face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
|
||||
facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
|
||||
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
float d = b3Dot(WorldNormal, separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for (int e0 = 0; e0 < numVertices; e0++)
|
||||
{
|
||||
const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
|
||||
worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
|
||||
}
|
||||
}
|
||||
|
||||
int closestFaceA = -1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for (int face = 0; face < hullA->m_numFaces; face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeFloat4(
|
||||
facesA[hullA->m_faceOffset + face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset + face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset + face].m_plane.z,
|
||||
0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
|
||||
|
||||
float d = b3Dot(faceANormalWS, separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
worldNormalsA1[pairIndex] = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
|
||||
for (int e0 = 0; e0 < numVerticesA; e0++)
|
||||
{
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
|
||||
worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
|
||||
}
|
||||
|
||||
clippingFaces[pairIndex].x = closestFaceA;
|
||||
clippingFaces[pairIndex].y = closestFaceB;
|
||||
clippingFaces[pairIndex].z = numVerticesA;
|
||||
clippingFaces[pairIndex].w = numWorldVertsB1;
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
__kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global const b3ConvexPolyhedronData* convexShapes,
|
||||
__global const b3Float4* vertices,
|
||||
__global const b3Float4* uniqueEdges,
|
||||
__global const b3GpuFace* faces,
|
||||
__global const int* indices,
|
||||
__global const b3GpuChildShape* gpuChildShapes,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Float4* concaveSeparatingNormalsOut,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Vector3* worldVertsA1Out,
|
||||
__global b3Vector3* worldNormalsA1Out,
|
||||
__global b3Vector3* worldVertsB1Out,
|
||||
__global int* hasSeparatingNormals,
|
||||
int vertexFaceCapacity,
|
||||
int numConcavePairs,
|
||||
int pairIdx)
|
||||
{
|
||||
int i = pairIdx;
|
||||
/* int i = get_global_id(0);
|
||||
if (i>=numConcavePairs)
|
||||
return;
|
||||
int pairIdx = i;
|
||||
*/
|
||||
|
||||
int bodyIndexA = concavePairs[i].x;
|
||||
int bodyIndexB = concavePairs[i].y;
|
||||
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
||||
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
||||
|
||||
if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
|
||||
collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
concavePairs[pairIdx].w = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
hasSeparatingNormals[i] = 0;
|
||||
|
||||
// int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
||||
int numActualConcaveConvexTests = 0;
|
||||
|
||||
int f = concavePairs[i].z;
|
||||
|
||||
bool overlap = false;
|
||||
|
||||
b3ConvexPolyhedronData convexPolyhedronA;
|
||||
|
||||
//add 3 vertices of the triangle
|
||||
convexPolyhedronA.m_numVertices = 3;
|
||||
convexPolyhedronA.m_vertexOffset = 0;
|
||||
b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
|
||||
b3Aabb triAabb;
|
||||
triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
|
||||
triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
|
||||
|
||||
b3Float4 verticesA[3];
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
int index = indices[face.m_indexOffset + i];
|
||||
b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
|
||||
verticesA[i] = vert;
|
||||
localCenter += vert;
|
||||
|
||||
triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
|
||||
triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
|
||||
}
|
||||
|
||||
overlap = true;
|
||||
overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
|
||||
overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
|
||||
overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
|
||||
|
||||
if (overlap)
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
int hasSeparatingAxis = 5;
|
||||
b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
|
||||
|
||||
// int localCC=0;
|
||||
numActualConcaveConvexTests++;
|
||||
|
||||
//a triangle has 3 unique edges
|
||||
convexPolyhedronA.m_numUniqueEdges = 3;
|
||||
convexPolyhedronA.m_uniqueEdgesOffset = 0;
|
||||
b3Float4 uniqueEdgesA[3];
|
||||
|
||||
uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
|
||||
uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
|
||||
uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
|
||||
|
||||
convexPolyhedronA.m_faceOffset = 0;
|
||||
|
||||
b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
|
||||
|
||||
b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
|
||||
int indicesA[3 + 3 + 2 + 2 + 2];
|
||||
int curUsedIndices = 0;
|
||||
int fidx = 0;
|
||||
|
||||
//front size of triangle
|
||||
{
|
||||
facesA[fidx].m_indexOffset = curUsedIndices;
|
||||
indicesA[0] = 0;
|
||||
indicesA[1] = 1;
|
||||
indicesA[2] = 2;
|
||||
curUsedIndices += 3;
|
||||
float c = face.m_plane.w;
|
||||
facesA[fidx].m_plane.x = normal.x;
|
||||
facesA[fidx].m_plane.y = normal.y;
|
||||
facesA[fidx].m_plane.z = normal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
facesA[fidx].m_numIndices = 3;
|
||||
}
|
||||
fidx++;
|
||||
//back size of triangle
|
||||
{
|
||||
facesA[fidx].m_indexOffset = curUsedIndices;
|
||||
indicesA[3] = 2;
|
||||
indicesA[4] = 1;
|
||||
indicesA[5] = 0;
|
||||
curUsedIndices += 3;
|
||||
float c = b3Dot(normal, verticesA[0]);
|
||||
// float c1 = -face.m_plane.w;
|
||||
facesA[fidx].m_plane.x = -normal.x;
|
||||
facesA[fidx].m_plane.y = -normal.y;
|
||||
facesA[fidx].m_plane.z = -normal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
facesA[fidx].m_numIndices = 3;
|
||||
}
|
||||
fidx++;
|
||||
|
||||
bool addEdgePlanes = true;
|
||||
if (addEdgePlanes)
|
||||
{
|
||||
int numVertices = 3;
|
||||
int prevVertex = numVertices - 1;
|
||||
for (int i = 0; i < numVertices; i++)
|
||||
{
|
||||
b3Float4 v0 = verticesA[i];
|
||||
b3Float4 v1 = verticesA[prevVertex];
|
||||
|
||||
b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
|
||||
float c = -b3Dot(edgeNormal, v0);
|
||||
|
||||
facesA[fidx].m_numIndices = 2;
|
||||
facesA[fidx].m_indexOffset = curUsedIndices;
|
||||
indicesA[curUsedIndices++] = i;
|
||||
indicesA[curUsedIndices++] = prevVertex;
|
||||
|
||||
facesA[fidx].m_plane.x = edgeNormal.x;
|
||||
facesA[fidx].m_plane.y = edgeNormal.y;
|
||||
facesA[fidx].m_plane.z = edgeNormal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
fidx++;
|
||||
prevVertex = i;
|
||||
}
|
||||
}
|
||||
convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
|
||||
convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
|
||||
|
||||
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
posB.w = 0.f;
|
||||
|
||||
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
///////////////////
|
||||
///compound shape support
|
||||
|
||||
if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int compoundChild = concavePairs[pairIdx].w;
|
||||
int childShapeIndexB = compoundChild; //collidables[collidableIndexB].m_shapeIndex+compoundChild;
|
||||
int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
||||
b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
||||
b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
||||
b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
|
||||
b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
|
||||
posB = newPosB;
|
||||
ornB = newOrnB;
|
||||
shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
||||
}
|
||||
//////////////////
|
||||
|
||||
b3Float4 c0local = convexPolyhedronA.m_localCenter;
|
||||
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
||||
b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
||||
b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
|
||||
const b3Float4 DeltaC2 = c0 - c1;
|
||||
|
||||
bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
|
||||
posA, ornA,
|
||||
posB, ornB,
|
||||
DeltaC2,
|
||||
verticesA, uniqueEdgesA, facesA, indicesA,
|
||||
vertices, uniqueEdges, faces, indices,
|
||||
&sepAxis, &dmin);
|
||||
hasSeparatingAxis = 4;
|
||||
if (!sepA)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
|
||||
posB, ornB,
|
||||
posA, ornA,
|
||||
DeltaC2,
|
||||
vertices, uniqueEdges, faces, indices,
|
||||
verticesA, uniqueEdgesA, facesA, indicesA,
|
||||
&sepAxis, &dmin);
|
||||
|
||||
if (!sepB)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
|
||||
posA, ornA,
|
||||
posB, ornB,
|
||||
DeltaC2,
|
||||
verticesA, uniqueEdgesA, facesA, indicesA,
|
||||
vertices, uniqueEdges, faces, indices,
|
||||
&sepAxis, &dmin, true);
|
||||
|
||||
if (!sepEE)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
hasSeparatingAxis = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (hasSeparatingAxis)
|
||||
{
|
||||
hasSeparatingNormals[i] = 1;
|
||||
sepAxis.w = dmin;
|
||||
concaveSeparatingNormalsOut[pairIdx] = sepAxis;
|
||||
|
||||
//now compute clipping faces A and B, and world-space clipping vertices A and B...
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
b3FindClippingFaces(sepAxis,
|
||||
&convexPolyhedronA,
|
||||
&convexShapes[shapeIndexB],
|
||||
posA, ornA,
|
||||
posB, ornB,
|
||||
worldVertsA1Out,
|
||||
worldNormalsA1Out,
|
||||
worldVertsB1Out,
|
||||
vertexFaceCapacity,
|
||||
minDist, maxDist,
|
||||
verticesA,
|
||||
facesA,
|
||||
indicesA,
|
||||
|
||||
vertices,
|
||||
faces,
|
||||
indices,
|
||||
clippingFacesOut, pairIdx);
|
||||
}
|
||||
else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
concavePairs[pairIdx].w = -1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
concavePairs[pairIdx].w = -1;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
|
|
@ -1,197 +0,0 @@
|
|||
#ifndef B3_FIND_SEPARATING_AXIS_H
|
||||
#define B3_FIND_SEPARATING_AXIS_H
|
||||
|
||||
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
|
||||
{
|
||||
min = FLT_MAX;
|
||||
max = -FLT_MAX;
|
||||
int numVerts = hull.m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
|
||||
|
||||
b3Scalar offset = b3Dot3F4(pos, dir);
|
||||
|
||||
for (int i = 0; i < numVerts; i++)
|
||||
{
|
||||
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
|
||||
//b3Scalar dp = pt.dot(dir);
|
||||
//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
|
||||
b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
|
||||
//b3Assert(dp==dpL);
|
||||
if (dp < min) min = dp;
|
||||
if (dp > max) max = dp;
|
||||
}
|
||||
if (min > max)
|
||||
{
|
||||
b3Scalar tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
}
|
||||
min += offset;
|
||||
max += offset;
|
||||
}
|
||||
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA, const b3Quaternion& ornA,
|
||||
const b3Float4& posB, const b3Quaternion& ornB,
|
||||
const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
|
||||
{
|
||||
b3Scalar Min0, Max0;
|
||||
b3Scalar Min1, Max1;
|
||||
b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
|
||||
b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
|
||||
|
||||
if (Max0 < Min1 || Max1 < Min0)
|
||||
return false;
|
||||
|
||||
b3Scalar d0 = Max0 - Min1;
|
||||
b3Assert(d0 >= 0.0f);
|
||||
b3Scalar d1 = Max1 - Min0;
|
||||
b3Assert(d1 >= 0.0f);
|
||||
depth = d0 < d1 ? d0 : d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
|
||||
const b3Float4& posA1,
|
||||
const b3Quaternion& ornA,
|
||||
const b3Float4& posB1,
|
||||
const b3Quaternion& ornB,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesA,
|
||||
const b3AlignedObjectArray<int>& indicesA,
|
||||
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
||||
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
|
||||
const b3AlignedObjectArray<b3GpuFace>& facesB,
|
||||
const b3AlignedObjectArray<int>& indicesB,
|
||||
|
||||
b3Vector3& sep)
|
||||
{
|
||||
B3_PROFILE("findSeparatingAxis");
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
//#ifdef TEST_INTERNAL_OBJECTS
|
||||
b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
|
||||
|
||||
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
||||
b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
|
||||
b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
|
||||
const b3Float4 deltaC2 = c0 - c1;
|
||||
//#endif
|
||||
|
||||
b3Scalar dmin = FLT_MAX;
|
||||
int curPlaneTests = 0;
|
||||
|
||||
int numFacesA = hullA.m_numFaces;
|
||||
// Test normals from hullA
|
||||
for (int i = 0; i < numFacesA; i++)
|
||||
{
|
||||
const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
|
||||
faceANormalWS *= -1.f;
|
||||
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar d;
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
|
||||
return false;
|
||||
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)faceANormalWS;
|
||||
}
|
||||
}
|
||||
|
||||
int numFacesB = hullB.m_numFaces;
|
||||
// Test normals from hullB
|
||||
for (int i = 0; i < numFacesB; i++)
|
||||
{
|
||||
b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
|
||||
b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
|
||||
|
||||
if (b3Dot3F4(deltaC2, WorldNormal) < 0)
|
||||
{
|
||||
WorldNormal *= -1.f;
|
||||
}
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar d;
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
|
||||
return false;
|
||||
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
sep = (b3Vector3&)WorldNormal;
|
||||
}
|
||||
}
|
||||
|
||||
// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
|
||||
{
|
||||
const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
|
||||
|
||||
for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
|
||||
{
|
||||
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
|
||||
|
||||
b3Float4 crossje = b3Cross3(edge0World, edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if (!b3IsAlmostZero((b3Vector3&)crossje))
|
||||
{
|
||||
crossje = b3FastNormalized3(crossje);
|
||||
if (b3Dot3F4(deltaC2, crossje) < 0)
|
||||
crossje *= -1.f;
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
b3Scalar dist;
|
||||
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
|
||||
return false;
|
||||
|
||||
if (dist < dmin)
|
||||
{
|
||||
dmin = dist;
|
||||
sep = (b3Vector3&)crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
|
||||
sep = -sep;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif //B3_FIND_SEPARATING_AXIS_H
|
||||
|
|
@ -1,888 +0,0 @@
|
|||
|
||||
/***
|
||||
* ---------------------------------
|
||||
* Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
|
||||
*
|
||||
* This file was ported from mpr.c file, part of libccd.
|
||||
* The Minkoski Portal Refinement implementation was ported
|
||||
* to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
|
||||
* at http://github.com/erwincoumans/bullet3
|
||||
*
|
||||
* Distributed under the OSI-approved BSD License (the "License");
|
||||
* see <http://www.opensource.org/licenses/bsd-license.php>.
|
||||
* This software is distributed WITHOUT ANY WARRANTY; without even the
|
||||
* implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the License for more information.
|
||||
*/
|
||||
|
||||
#ifndef B3_MPR_PENETRATION_H
|
||||
#define B3_MPR_PENETRATION_H
|
||||
|
||||
#include "Bullet3Common/shared/b3PlatformDefinitions.h"
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define B3_MPR_SQRT sqrtf
|
||||
#else
|
||||
#define B3_MPR_SQRT sqrt
|
||||
#endif
|
||||
#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
|
||||
#define B3_MPR_FABS fabs
|
||||
|
||||
#define B3_MPR_TOLERANCE 1E-6f
|
||||
#define B3_MPR_MAX_ITERATIONS 1000
|
||||
|
||||
struct _b3MprSupport_t
|
||||
{
|
||||
b3Float4 v; //!< Support point in minkowski sum
|
||||
b3Float4 v1; //!< Support point in obj1
|
||||
b3Float4 v2; //!< Support point in obj2
|
||||
};
|
||||
typedef struct _b3MprSupport_t b3MprSupport_t;
|
||||
|
||||
struct _b3MprSimplex_t
|
||||
{
|
||||
b3MprSupport_t ps[4];
|
||||
int last; //!< index of last added point
|
||||
};
|
||||
typedef struct _b3MprSimplex_t b3MprSimplex_t;
|
||||
|
||||
inline b3MprSupport_t *b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
|
||||
{
|
||||
return &s->ps[idx];
|
||||
}
|
||||
|
||||
inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
|
||||
{
|
||||
s->last = size - 1;
|
||||
}
|
||||
|
||||
inline int b3MprSimplexSize(const b3MprSimplex_t *s)
|
||||
{
|
||||
return s->last + 1;
|
||||
}
|
||||
|
||||
inline const b3MprSupport_t *b3MprSimplexPoint(const b3MprSimplex_t *s, int idx)
|
||||
{
|
||||
// here is no check on boundaries
|
||||
return &s->ps[idx];
|
||||
}
|
||||
|
||||
inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
|
||||
{
|
||||
*d = *s;
|
||||
}
|
||||
|
||||
inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
|
||||
{
|
||||
b3MprSupportCopy(s->ps + pos, a);
|
||||
}
|
||||
|
||||
inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
|
||||
{
|
||||
b3MprSupport_t supp;
|
||||
|
||||
b3MprSupportCopy(&supp, &s->ps[pos1]);
|
||||
b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
|
||||
b3MprSupportCopy(&s->ps[pos2], &supp);
|
||||
}
|
||||
|
||||
inline int b3MprIsZero(float val)
|
||||
{
|
||||
return B3_MPR_FABS(val) < FLT_EPSILON;
|
||||
}
|
||||
|
||||
inline int b3MprEq(float _a, float _b)
|
||||
{
|
||||
float ab;
|
||||
float a, b;
|
||||
|
||||
ab = B3_MPR_FABS(_a - _b);
|
||||
if (B3_MPR_FABS(ab) < FLT_EPSILON)
|
||||
return 1;
|
||||
|
||||
a = B3_MPR_FABS(_a);
|
||||
b = B3_MPR_FABS(_b);
|
||||
if (b > a)
|
||||
{
|
||||
return ab < FLT_EPSILON * b;
|
||||
}
|
||||
else
|
||||
{
|
||||
return ab < FLT_EPSILON * a;
|
||||
}
|
||||
}
|
||||
|
||||
inline int b3MprVec3Eq(const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
return b3MprEq((*a).x, (*b).x) && b3MprEq((*a).y, (*b).y) && b3MprEq((*a).z, (*b).z);
|
||||
}
|
||||
|
||||
inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec, __global const b3ConvexPolyhedronData_t *hull, b3ConstArray(b3Float4) verticesA)
|
||||
{
|
||||
b3Float4 supVec = b3MakeFloat4(0, 0, 0, 0);
|
||||
float maxDot = -B3_LARGE_FLOAT;
|
||||
|
||||
if (0 < hull->m_numVertices)
|
||||
{
|
||||
const b3Float4 scaled = supportVec;
|
||||
int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
|
||||
return verticesA[hull->m_vertexOffset + index];
|
||||
}
|
||||
|
||||
return supVec;
|
||||
}
|
||||
|
||||
B3_STATIC void b3MprConvexSupport(int pairIndex, int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4 *sepAxis,
|
||||
const b3Float4 *_dir, b3Float4 *outp, int logme)
|
||||
{
|
||||
//dir is in worldspace, move to local space
|
||||
|
||||
b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
|
||||
b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
|
||||
|
||||
b3Float4 dir = b3MakeFloat4((*_dir).x, (*_dir).y, (*_dir).z, 0.f);
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), dir);
|
||||
|
||||
//find local support vertex
|
||||
int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
|
||||
|
||||
b3Assert(cpuCollidables[colIndex].m_shapeType == SHAPE_CONVEX_HULL);
|
||||
__global const b3ConvexPolyhedronData_t *hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
|
||||
|
||||
b3Float4 pInA;
|
||||
if (logme)
|
||||
{
|
||||
// b3Float4 supVec = b3MakeFloat4(0,0,0,0);
|
||||
float maxDot = -B3_LARGE_FLOAT;
|
||||
|
||||
if (0 < hull->m_numVertices)
|
||||
{
|
||||
const b3Float4 scaled = localDir;
|
||||
int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
|
||||
pInA = cpuVertices[hull->m_vertexOffset + index];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pInA = b3LocalGetSupportVertex(localDir, hull, cpuVertices);
|
||||
}
|
||||
|
||||
//move vertex to world space
|
||||
*outp = b3TransformPoint(pInA, pos, orn);
|
||||
}
|
||||
|
||||
inline void b3MprSupport(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4 *sepAxis,
|
||||
const b3Float4 *_dir, b3MprSupport_t *supp)
|
||||
{
|
||||
b3Float4 dir;
|
||||
dir = *_dir;
|
||||
b3MprConvexSupport(pairIndex, bodyIndexA, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v1, 0);
|
||||
dir = *_dir * -1.f;
|
||||
b3MprConvexSupport(pairIndex, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v2, 0);
|
||||
supp->v = supp->v1 - supp->v2;
|
||||
}
|
||||
|
||||
inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
|
||||
{
|
||||
center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
|
||||
center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
|
||||
center->v = center->v1 - center->v2;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
|
||||
{
|
||||
(*v).x = x;
|
||||
(*v).y = y;
|
||||
(*v).z = z;
|
||||
(*v).w = 0.f;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
|
||||
{
|
||||
(*v).x += (*w).x;
|
||||
(*v).y += (*w).y;
|
||||
(*v).z += (*w).z;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
|
||||
{
|
||||
*v = *w;
|
||||
}
|
||||
|
||||
inline void b3MprVec3Scale(b3Float4 *d, float k)
|
||||
{
|
||||
*d *= k;
|
||||
}
|
||||
|
||||
inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
float dot;
|
||||
|
||||
dot = b3Dot3F4(*a, *b);
|
||||
return dot;
|
||||
}
|
||||
|
||||
inline float b3MprVec3Len2(const b3Float4 *v)
|
||||
{
|
||||
return b3MprVec3Dot(v, v);
|
||||
}
|
||||
|
||||
inline void b3MprVec3Normalize(b3Float4 *d)
|
||||
{
|
||||
float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
|
||||
b3MprVec3Scale(d, k);
|
||||
}
|
||||
|
||||
inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
*d = b3Cross3(*a, *b);
|
||||
}
|
||||
|
||||
inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
|
||||
{
|
||||
*d = *v - *w;
|
||||
}
|
||||
|
||||
inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
|
||||
{
|
||||
b3Float4 v2v1, v3v1;
|
||||
|
||||
b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b3MprVec3Cross(dir, &v2v1, &v3v1);
|
||||
b3MprVec3Normalize(dir);
|
||||
}
|
||||
|
||||
inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
|
||||
const b3Float4 *dir)
|
||||
{
|
||||
float dot;
|
||||
dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
|
||||
return b3MprIsZero(dot) || dot > 0.f;
|
||||
}
|
||||
|
||||
inline int portalReachTolerance(const b3MprSimplex_t *portal,
|
||||
const b3MprSupport_t *v4,
|
||||
const b3Float4 *dir)
|
||||
{
|
||||
float dv1, dv2, dv3, dv4;
|
||||
float dot1, dot2, dot3;
|
||||
|
||||
// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
|
||||
|
||||
dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
|
||||
dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
|
||||
dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
|
||||
dv4 = b3MprVec3Dot(&v4->v, dir);
|
||||
|
||||
dot1 = dv4 - dv1;
|
||||
dot2 = dv4 - dv2;
|
||||
dot3 = dv4 - dv3;
|
||||
|
||||
dot1 = B3_MPR_FMIN(dot1, dot2);
|
||||
dot1 = B3_MPR_FMIN(dot1, dot3);
|
||||
|
||||
return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
|
||||
}
|
||||
|
||||
inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,
|
||||
const b3MprSupport_t *v4,
|
||||
const b3Float4 *dir)
|
||||
{
|
||||
float dot;
|
||||
dot = b3MprVec3Dot(&v4->v, dir);
|
||||
return b3MprIsZero(dot) || dot > 0.f;
|
||||
}
|
||||
|
||||
inline void b3ExpandPortal(b3MprSimplex_t *portal,
|
||||
const b3MprSupport_t *v4)
|
||||
{
|
||||
float dot;
|
||||
b3Float4 v4v0;
|
||||
|
||||
b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
|
||||
if (dot > 0.f)
|
||||
{
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
|
||||
if (dot > 0.f)
|
||||
{
|
||||
b3MprSimplexSet(portal, 1, v4);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3MprSimplexSet(portal, 3, v4);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
|
||||
if (dot > 0.f)
|
||||
{
|
||||
b3MprSimplexSet(portal, 2, v4);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3MprSimplexSet(portal, 1, v4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4 *sepAxis,
|
||||
__global int *hasSepAxis,
|
||||
b3MprSimplex_t *portal)
|
||||
{
|
||||
b3Float4 dir, va, vb;
|
||||
float dot;
|
||||
int cont;
|
||||
|
||||
// vertex 0 is center of portal
|
||||
b3FindOrigin(bodyIndexA, bodyIndexB, cpuBodyBuf, b3MprSimplexPointW(portal, 0));
|
||||
// vertex 0 is center of portal
|
||||
b3MprSimplexSetSize(portal, 1);
|
||||
|
||||
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
|
||||
b3Float4 *b3mpr_vec3_origin = &zero;
|
||||
|
||||
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin))
|
||||
{
|
||||
// Portal's center lies on origin (0,0,0) => we know that objects
|
||||
// intersect but we would need to know penetration info.
|
||||
// So move center little bit...
|
||||
b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
|
||||
b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
|
||||
}
|
||||
|
||||
// vertex 1 = support in direction of origin
|
||||
b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Scale(&dir, -1.f);
|
||||
b3MprVec3Normalize(&dir);
|
||||
|
||||
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 1));
|
||||
|
||||
b3MprSimplexSetSize(portal, 2);
|
||||
|
||||
// test if origin isn't outside of v1
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
|
||||
|
||||
if (b3MprIsZero(dot) || dot < 0.f)
|
||||
return -1;
|
||||
|
||||
// vertex 2
|
||||
b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
if (b3MprIsZero(b3MprVec3Len2(&dir)))
|
||||
{
|
||||
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin))
|
||||
{
|
||||
// origin lies on v1
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// origin lies on v0-v1 segment
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
|
||||
b3MprVec3Normalize(&dir);
|
||||
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 2));
|
||||
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
|
||||
if (b3MprIsZero(dot) || dot < 0.f)
|
||||
return -1;
|
||||
|
||||
b3MprSimplexSetSize(portal, 3);
|
||||
|
||||
// vertex 3 direction
|
||||
b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Cross(&dir, &va, &vb);
|
||||
b3MprVec3Normalize(&dir);
|
||||
|
||||
// it is better to form portal faces to be oriented "outside" origin
|
||||
dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
|
||||
if (dot > 0.f)
|
||||
{
|
||||
b3MprSimplexSwap(portal, 1, 2);
|
||||
b3MprVec3Scale(&dir, -1.f);
|
||||
}
|
||||
|
||||
while (b3MprSimplexSize(portal) < 4)
|
||||
{
|
||||
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 3));
|
||||
|
||||
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
|
||||
if (b3MprIsZero(dot) || dot < 0.f)
|
||||
return -1;
|
||||
|
||||
cont = 0;
|
||||
|
||||
// test if origin is outside (v1, v0, v3) - set v2 as v3 and
|
||||
// continue
|
||||
b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 3)->v);
|
||||
dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
|
||||
if (dot < 0.f && !b3MprIsZero(dot))
|
||||
{
|
||||
b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
|
||||
cont = 1;
|
||||
}
|
||||
|
||||
if (!cont)
|
||||
{
|
||||
// test if origin is outside (v3, v0, v2) - set v1 as v3 and
|
||||
// continue
|
||||
b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
|
||||
if (dot < 0.f && !b3MprIsZero(dot))
|
||||
{
|
||||
b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
|
||||
cont = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (cont)
|
||||
{
|
||||
b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 0)->v);
|
||||
b3MprVec3Cross(&dir, &va, &vb);
|
||||
b3MprVec3Normalize(&dir);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3MprSimplexSetSize(portal, 4);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_STATIC int b3RefinePortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4 *sepAxis,
|
||||
b3MprSimplex_t *portal)
|
||||
{
|
||||
b3Float4 dir;
|
||||
b3MprSupport_t v4;
|
||||
|
||||
for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
|
||||
//while (1)
|
||||
{
|
||||
// compute direction outside the portal (from v0 throught v1,v2,v3
|
||||
// face)
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
// test if origin is inside the portal
|
||||
if (portalEncapsulesOrigin(portal, &dir))
|
||||
return 0;
|
||||
|
||||
// get next support point
|
||||
|
||||
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
|
||||
|
||||
// test if v4 can expand portal to contain origin and if portal
|
||||
// expanding doesn't reach given tolerance
|
||||
if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// v1-v2-v3 triangle must be rearranged to face outside Minkowski
|
||||
// difference (direction from v0).
|
||||
b3ExpandPortal(portal, &v4);
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
|
||||
{
|
||||
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
|
||||
b3Float4 *b3mpr_vec3_origin = &zero;
|
||||
|
||||
b3Float4 dir;
|
||||
size_t i;
|
||||
float b[4], sum, inv;
|
||||
b3Float4 vec, p1, p2;
|
||||
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
// use barycentric coordinates of tetrahedron to find origin
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
|
||||
|
||||
sum = b[0] + b[1] + b[2] + b[3];
|
||||
|
||||
if (b3MprIsZero(sum) || sum < 0.f)
|
||||
{
|
||||
b[0] = 0.f;
|
||||
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
|
||||
&b3MprSimplexPoint(portal, 3)->v);
|
||||
b[1] = b3MprVec3Dot(&vec, &dir);
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
|
||||
&b3MprSimplexPoint(portal, 1)->v);
|
||||
b[2] = b3MprVec3Dot(&vec, &dir);
|
||||
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
|
||||
&b3MprSimplexPoint(portal, 2)->v);
|
||||
b[3] = b3MprVec3Dot(&vec, &dir);
|
||||
|
||||
sum = b[1] + b[2] + b[3];
|
||||
}
|
||||
|
||||
inv = 1.f / sum;
|
||||
|
||||
b3MprVec3Copy(&p1, b3mpr_vec3_origin);
|
||||
b3MprVec3Copy(&p2, b3mpr_vec3_origin);
|
||||
for (i = 0; i < 4; i++)
|
||||
{
|
||||
b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
|
||||
b3MprVec3Scale(&vec, b[i]);
|
||||
b3MprVec3Add(&p1, &vec);
|
||||
|
||||
b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
|
||||
b3MprVec3Scale(&vec, b[i]);
|
||||
b3MprVec3Add(&p2, &vec);
|
||||
}
|
||||
b3MprVec3Scale(&p1, inv);
|
||||
b3MprVec3Scale(&p2, inv);
|
||||
|
||||
b3MprVec3Copy(pos, &p1);
|
||||
b3MprVec3Add(pos, &p2);
|
||||
b3MprVec3Scale(pos, 0.5);
|
||||
}
|
||||
|
||||
inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
|
||||
{
|
||||
b3Float4 ab;
|
||||
b3MprVec3Sub2(&ab, a, b);
|
||||
return b3MprVec3Len2(&ab);
|
||||
}
|
||||
|
||||
inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
|
||||
const b3Float4 *x0,
|
||||
const b3Float4 *b,
|
||||
b3Float4 *witness)
|
||||
{
|
||||
// The computation comes from solving equation of segment:
|
||||
// S(t) = x0 + t.d
|
||||
// where - x0 is initial point of segment
|
||||
// - d is direction of segment from x0 (|d| > 0)
|
||||
// - t belongs to <0, 1> interval
|
||||
//
|
||||
// Than, distance from a segment to some point P can be expressed:
|
||||
// D(t) = |x0 + t.d - P|^2
|
||||
// which is distance from any point on segment. Minimization
|
||||
// of this function brings distance from P to segment.
|
||||
// Minimization of D(t) leads to simple quadratic equation that's
|
||||
// solving is straightforward.
|
||||
//
|
||||
// Bonus of this method is witness point for free.
|
||||
|
||||
float dist, t;
|
||||
b3Float4 d, a;
|
||||
|
||||
// direction of segment
|
||||
b3MprVec3Sub2(&d, b, x0);
|
||||
|
||||
// precompute vector from P to x0
|
||||
b3MprVec3Sub2(&a, x0, P);
|
||||
|
||||
t = -1.f * b3MprVec3Dot(&a, &d);
|
||||
t /= b3MprVec3Len2(&d);
|
||||
|
||||
if (t < 0.f || b3MprIsZero(t))
|
||||
{
|
||||
dist = b3MprVec3Dist2(x0, P);
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, x0);
|
||||
}
|
||||
else if (t > 1.f || b3MprEq(t, 1.f))
|
||||
{
|
||||
dist = b3MprVec3Dist2(b, P);
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, b);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (witness)
|
||||
{
|
||||
b3MprVec3Copy(witness, &d);
|
||||
b3MprVec3Scale(witness, t);
|
||||
b3MprVec3Add(witness, x0);
|
||||
dist = b3MprVec3Dist2(witness, P);
|
||||
}
|
||||
else
|
||||
{
|
||||
// recycling variables
|
||||
b3MprVec3Scale(&d, t);
|
||||
b3MprVec3Add(&d, &a);
|
||||
dist = b3MprVec3Len2(&d);
|
||||
}
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
inline float b3MprVec3PointTriDist2(const b3Float4 *P,
|
||||
const b3Float4 *x0, const b3Float4 *B,
|
||||
const b3Float4 *C,
|
||||
b3Float4 *witness)
|
||||
{
|
||||
// Computation comes from analytic expression for triangle (x0, B, C)
|
||||
// T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
|
||||
// Then equation for distance is:
|
||||
// D(s, t) = | T(s, t) - P |^2
|
||||
// This leads to minimization of quadratic function of two variables.
|
||||
// The solution from is taken only if s is between 0 and 1, t is
|
||||
// between 0 and 1 and t + s < 1, otherwise distance from segment is
|
||||
// computed.
|
||||
|
||||
b3Float4 d1, d2, a;
|
||||
float u, v, w, p, q, r;
|
||||
float s, t, dist, dist2;
|
||||
b3Float4 witness2;
|
||||
|
||||
b3MprVec3Sub2(&d1, B, x0);
|
||||
b3MprVec3Sub2(&d2, C, x0);
|
||||
b3MprVec3Sub2(&a, x0, P);
|
||||
|
||||
u = b3MprVec3Dot(&a, &a);
|
||||
v = b3MprVec3Dot(&d1, &d1);
|
||||
w = b3MprVec3Dot(&d2, &d2);
|
||||
p = b3MprVec3Dot(&a, &d1);
|
||||
q = b3MprVec3Dot(&a, &d2);
|
||||
r = b3MprVec3Dot(&d1, &d2);
|
||||
|
||||
s = (q * r - w * p) / (w * v - r * r);
|
||||
t = (-s * r - q) / w;
|
||||
|
||||
if ((b3MprIsZero(s) || s > 0.f) && (b3MprEq(s, 1.f) || s < 1.f) && (b3MprIsZero(t) || t > 0.f) && (b3MprEq(t, 1.f) || t < 1.f) && (b3MprEq(t + s, 1.f) || t + s < 1.f))
|
||||
{
|
||||
if (witness)
|
||||
{
|
||||
b3MprVec3Scale(&d1, s);
|
||||
b3MprVec3Scale(&d2, t);
|
||||
b3MprVec3Copy(witness, x0);
|
||||
b3MprVec3Add(witness, &d1);
|
||||
b3MprVec3Add(witness, &d2);
|
||||
|
||||
dist = b3MprVec3Dist2(witness, P);
|
||||
}
|
||||
else
|
||||
{
|
||||
dist = s * s * v;
|
||||
dist += t * t * w;
|
||||
dist += 2.f * s * t * r;
|
||||
dist += 2.f * s * p;
|
||||
dist += 2.f * t * q;
|
||||
dist += u;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
|
||||
|
||||
dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
|
||||
if (dist2 < dist)
|
||||
{
|
||||
dist = dist2;
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, &witness2);
|
||||
}
|
||||
|
||||
dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
|
||||
if (dist2 < dist)
|
||||
{
|
||||
dist = dist2;
|
||||
if (witness)
|
||||
b3MprVec3Copy(witness, &witness2);
|
||||
}
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPenetr(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4 *sepAxis,
|
||||
b3MprSimplex_t *portal,
|
||||
float *depth, b3Float4 *pdir, b3Float4 *pos)
|
||||
{
|
||||
b3Float4 dir;
|
||||
b3MprSupport_t v4;
|
||||
unsigned long iterations;
|
||||
|
||||
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
|
||||
b3Float4 *b3mpr_vec3_origin = &zero;
|
||||
|
||||
iterations = 1UL;
|
||||
for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
|
||||
//while (1)
|
||||
{
|
||||
// compute portal direction and obtain next support point
|
||||
b3PortalDir(portal, &dir);
|
||||
|
||||
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
|
||||
|
||||
// reached tolerance -> find penetration info
|
||||
if (portalReachTolerance(portal, &v4, &dir) || iterations == B3_MPR_MAX_ITERATIONS)
|
||||
{
|
||||
*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin, &b3MprSimplexPoint(portal, 1)->v, &b3MprSimplexPoint(portal, 2)->v, &b3MprSimplexPoint(portal, 3)->v, pdir);
|
||||
*depth = B3_MPR_SQRT(*depth);
|
||||
|
||||
if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
|
||||
{
|
||||
*pdir = dir;
|
||||
}
|
||||
b3MprVec3Normalize(pdir);
|
||||
|
||||
// barycentric coordinates:
|
||||
b3FindPos(portal, pos);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
b3ExpandPortal(portal, &v4);
|
||||
|
||||
iterations++;
|
||||
}
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal, float *depth, b3Float4 *dir, b3Float4 *pos)
|
||||
{
|
||||
// Touching contact on portal's v1 - so depth is zero and direction
|
||||
// is unimportant and pos can be guessed
|
||||
*depth = 0.f;
|
||||
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
|
||||
b3Float4 *b3mpr_vec3_origin = &zero;
|
||||
|
||||
b3MprVec3Copy(dir, b3mpr_vec3_origin);
|
||||
|
||||
b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
|
||||
b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
|
||||
b3MprVec3Scale(pos, 0.5);
|
||||
}
|
||||
|
||||
B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
|
||||
float *depth, b3Float4 *dir, b3Float4 *pos)
|
||||
{
|
||||
// Origin lies on v0-v1 segment.
|
||||
// Depth is distance to v1, direction also and position must be
|
||||
// computed
|
||||
|
||||
b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
|
||||
b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
|
||||
b3MprVec3Scale(pos, 0.5f);
|
||||
|
||||
b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
|
||||
*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
|
||||
b3MprVec3Normalize(dir);
|
||||
}
|
||||
|
||||
inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
|
||||
b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
|
||||
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
|
||||
b3ConstArray(b3Collidable_t) cpuCollidables,
|
||||
b3ConstArray(b3Float4) cpuVertices,
|
||||
__global b3Float4 *sepAxis,
|
||||
__global int *hasSepAxis,
|
||||
float *depthOut, b3Float4 *dirOut, b3Float4 *posOut)
|
||||
{
|
||||
b3MprSimplex_t portal;
|
||||
|
||||
// if (!hasSepAxis[pairIndex])
|
||||
// return -1;
|
||||
|
||||
hasSepAxis[pairIndex] = 0;
|
||||
int res;
|
||||
|
||||
// Phase 1: Portal discovery
|
||||
res = b3DiscoverPortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, hasSepAxis, &portal);
|
||||
|
||||
//sepAxis[pairIndex] = *pdir;//or -dir?
|
||||
|
||||
switch (res)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
// Phase 2: Portal refinement
|
||||
|
||||
res = b3RefinePortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal);
|
||||
if (res < 0)
|
||||
return -1;
|
||||
|
||||
// Phase 3. Penetration info
|
||||
b3FindPenetr(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal, depthOut, dirOut, posOut);
|
||||
hasSepAxis[pairIndex] = 1;
|
||||
sepAxis[pairIndex] = -*dirOut;
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
// Touching contact on portal's v1.
|
||||
b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
b3FindPenetrSegment(&portal, depthOut, dirOut, posOut);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
hasSepAxis[pairIndex] = 0;
|
||||
//if (res < 0)
|
||||
//{
|
||||
// Origin isn't inside portal - no collision.
|
||||
return -1;
|
||||
//}
|
||||
}
|
||||
};
|
||||
|
||||
return 0;
|
||||
};
|
||||
|
||||
#endif //B3_MPR_PENETRATION_H
|
||||
|
|
@ -1,175 +0,0 @@
|
|||
|
||||
#ifndef B3_NEW_CONTACT_REDUCTION_H
|
||||
#define B3_NEW_CONTACT_REDUCTION_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
|
||||
#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
|
||||
|
||||
int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
|
||||
{
|
||||
if (nPoints == 0)
|
||||
return 0;
|
||||
|
||||
if (nPoints <= 4)
|
||||
return nPoints;
|
||||
|
||||
if (nPoints > 64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
|
||||
{
|
||||
for (int i = 0; i < nPoints; i++)
|
||||
center += p[i];
|
||||
center /= (float)nPoints;
|
||||
}
|
||||
|
||||
// sample 4 directions
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross(nearNormal, aVector);
|
||||
b3Float4 v = b3Cross(nearNormal, u);
|
||||
u = b3Normalized(u);
|
||||
v = b3Normalized(v);
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW = FLT_MAX;
|
||||
|
||||
int minIndex = -1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for (int ie = 0; ie < nPoints; ie++)
|
||||
{
|
||||
if (p[ie].w < minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex = ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie] - center;
|
||||
f = b3Dot(u, r);
|
||||
if (f < maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot(-u, r);
|
||||
if (f < maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
f = b3Dot(v, r);
|
||||
if (f < maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot(-v, r);
|
||||
if (f < maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
}
|
||||
|
||||
__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
|
||||
__global const b3RigidBodyData_t* rigidBodies,
|
||||
__global const b3Float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global struct b3Contact4Data* globalContactsOut,
|
||||
__global b3Int4* clippingFaces,
|
||||
__global b3Float4* worldVertsB2,
|
||||
volatile __global int* nGlobalContactsOut,
|
||||
int vertexFaceCapacity,
|
||||
int contactCapacity,
|
||||
int numPairs,
|
||||
int pairIndex)
|
||||
{
|
||||
// int i = get_global_id(0);
|
||||
//int pairIndex = i;
|
||||
int i = pairIndex;
|
||||
|
||||
b3Int4 contactIdx;
|
||||
contactIdx = b3MakeInt4(0, 1, 2, 3);
|
||||
|
||||
if (i < numPairs)
|
||||
{
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
int nPoints = clippingFaces[pairIndex].w;
|
||||
|
||||
if (nPoints > 0)
|
||||
{
|
||||
__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
|
||||
b3Float4 normal = -separatingNormals[i];
|
||||
|
||||
int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
|
||||
|
||||
int dstIdx;
|
||||
dstIdx = b3AtomicInc(nGlobalContactsOut);
|
||||
|
||||
//#if 0
|
||||
b3Assert(dstIdx < contactCapacity);
|
||||
if (dstIdx < contactCapacity)
|
||||
{
|
||||
__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormalOnB = -normal;
|
||||
c->m_restituitionCoeffCmp = (0.f * 0xffff);
|
||||
c->m_frictionCoeffCmp = (0.7f * 0xffff);
|
||||
c->m_batchIdx = pairIndex;
|
||||
int bodyA = pairs[pairIndex].x;
|
||||
int bodyB = pairs[pairIndex].y;
|
||||
|
||||
pairs[pairIndex].w = dstIdx;
|
||||
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
|
||||
c->m_childIndexA = -1;
|
||||
c->m_childIndexB = -1;
|
||||
|
||||
switch (nReducedContacts)
|
||||
{
|
||||
case 4:
|
||||
c->m_worldPosB[3] = pointsIn[contactIdx.w];
|
||||
case 3:
|
||||
c->m_worldPosB[2] = pointsIn[contactIdx.z];
|
||||
case 2:
|
||||
c->m_worldPosB[1] = pointsIn[contactIdx.y];
|
||||
case 1:
|
||||
c->m_worldPosB[0] = pointsIn[contactIdx.x];
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
GET_NPOINTS(*c) = nReducedContacts;
|
||||
}
|
||||
|
||||
//#endif
|
||||
|
||||
} // if (numContactsOut>0)
|
||||
} // if (hasSeparatingAxis[i])
|
||||
} // if (i<numPairs)
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,88 +0,0 @@
|
|||
|
||||
|
||||
#ifndef B3_QUANTIZED_BVH_NODE_H
|
||||
#define B3_QUANTIZED_BVH_NODE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
#define B3_MAX_NUM_PARTS_IN_BITS 10
|
||||
|
||||
///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
|
||||
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
|
||||
typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
|
||||
|
||||
struct b3QuantizedBvhNodeData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
};
|
||||
|
||||
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
unsigned int x = 0;
|
||||
unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
|
||||
// Get only the lower bits where the triangle index is stored
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
|
||||
}
|
||||
|
||||
inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
|
||||
}
|
||||
|
||||
inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
return -rootNode->m_escapeIndexOrTriangleIndex;
|
||||
}
|
||||
|
||||
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
|
||||
{
|
||||
b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
|
||||
clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
|
||||
|
||||
b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
|
||||
if (isMax)
|
||||
{
|
||||
out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
|
||||
out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
|
||||
out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
|
||||
}
|
||||
else
|
||||
{
|
||||
out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
|
||||
out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
|
||||
out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
|
||||
}
|
||||
}
|
||||
|
||||
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
|
||||
const unsigned short int* aabbMin1,
|
||||
const unsigned short int* aabbMax1,
|
||||
const unsigned short int* aabbMin2,
|
||||
const unsigned short int* aabbMax2)
|
||||
{
|
||||
//int overlap = 1;
|
||||
if (aabbMin1[0] > aabbMax2[0])
|
||||
return 0;
|
||||
if (aabbMax1[0] < aabbMin2[0])
|
||||
return 0;
|
||||
if (aabbMin1[1] > aabbMax2[1])
|
||||
return 0;
|
||||
if (aabbMax1[1] < aabbMin2[1])
|
||||
return 0;
|
||||
if (aabbMin1[2] > aabbMax2[2])
|
||||
return 0;
|
||||
if (aabbMax1[2] < aabbMin2[2])
|
||||
return 0;
|
||||
return 1;
|
||||
//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
|
||||
//return overlap;
|
||||
}
|
||||
|
||||
#endif //B3_QUANTIZED_BVH_NODE_H
|
||||
|
|
@ -1,89 +0,0 @@
|
|||
#ifndef B3_REDUCE_CONTACTS_H
|
||||
#define B3_REDUCE_CONTACTS_H
|
||||
|
||||
inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
|
||||
{
|
||||
if (nPoints == 0)
|
||||
return 0;
|
||||
|
||||
if (nPoints <= 4)
|
||||
return nPoints;
|
||||
|
||||
if (nPoints > 64)
|
||||
nPoints = 64;
|
||||
|
||||
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
|
||||
{
|
||||
for (int i = 0; i < nPoints; i++)
|
||||
center += p[i];
|
||||
center /= (float)nPoints;
|
||||
}
|
||||
|
||||
// sample 4 directions
|
||||
|
||||
b3Float4 aVector = p[0] - center;
|
||||
b3Float4 u = b3Cross3(nearNormal, aVector);
|
||||
b3Float4 v = b3Cross3(nearNormal, u);
|
||||
u = b3FastNormalized3(u);
|
||||
v = b3FastNormalized3(v);
|
||||
|
||||
//keep point with deepest penetration
|
||||
float minW = FLT_MAX;
|
||||
|
||||
int minIndex = -1;
|
||||
|
||||
b3Float4 maxDots;
|
||||
maxDots.x = FLT_MIN;
|
||||
maxDots.y = FLT_MIN;
|
||||
maxDots.z = FLT_MIN;
|
||||
maxDots.w = FLT_MIN;
|
||||
|
||||
// idx, distance
|
||||
for (int ie = 0; ie < nPoints; ie++)
|
||||
{
|
||||
if (p[ie].w < minW)
|
||||
{
|
||||
minW = p[ie].w;
|
||||
minIndex = ie;
|
||||
}
|
||||
float f;
|
||||
b3Float4 r = p[ie] - center;
|
||||
f = b3Dot3F4(u, r);
|
||||
if (f < maxDots.x)
|
||||
{
|
||||
maxDots.x = f;
|
||||
contactIdx[0].x = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4(-u, r);
|
||||
if (f < maxDots.y)
|
||||
{
|
||||
maxDots.y = f;
|
||||
contactIdx[0].y = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4(v, r);
|
||||
if (f < maxDots.z)
|
||||
{
|
||||
maxDots.z = f;
|
||||
contactIdx[0].z = ie;
|
||||
}
|
||||
|
||||
f = b3Dot3F4(-v, r);
|
||||
if (f < maxDots.w)
|
||||
{
|
||||
maxDots.w = f;
|
||||
contactIdx[0].w = ie;
|
||||
}
|
||||
}
|
||||
|
||||
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
|
||||
{
|
||||
//replace the first contact with minimum (todo: replace contact with least penetration)
|
||||
contactIdx[0].x = minIndex;
|
||||
}
|
||||
|
||||
return 4;
|
||||
}
|
||||
|
||||
#endif //B3_REDUCE_CONTACTS_H
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
#ifndef B3_RIGIDBODY_DATA_H
|
||||
#define B3_RIGIDBODY_DATA_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
#include "Bullet3Common/shared/b3Mat3x3.h"
|
||||
|
||||
typedef struct b3RigidBodyData b3RigidBodyData_t;
|
||||
|
||||
struct b3RigidBodyData
|
||||
{
|
||||
b3Float4 m_pos;
|
||||
b3Quat m_quat;
|
||||
b3Float4 m_linVel;
|
||||
b3Float4 m_angVel;
|
||||
|
||||
int m_collidableIdx;
|
||||
float m_invMass;
|
||||
float m_restituitionCoeff;
|
||||
float m_frictionCoeff;
|
||||
};
|
||||
|
||||
typedef struct b3InertiaData b3InertiaData_t;
|
||||
|
||||
struct b3InertiaData
|
||||
{
|
||||
b3Mat3x3 m_invInertiaWorld;
|
||||
b3Mat3x3 m_initInvInertia;
|
||||
};
|
||||
|
||||
#endif //B3_RIGIDBODY_DATA_H
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
#ifndef B3_UPDATE_AABBS_H
|
||||
#define B3_UPDATE_AABBS_H
|
||||
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
|
||||
{
|
||||
__global const b3RigidBodyData_t* body = &bodies[bodyId];
|
||||
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
|
||||
|
||||
if (shapeIndex >= 0)
|
||||
{
|
||||
b3Aabb_t localAabb = localShapeAABB[collidableIndex];
|
||||
b3Aabb_t worldAabb;
|
||||
|
||||
b3Float4 aabbAMinOut, aabbAMaxOut;
|
||||
float margin = 0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
|
||||
|
||||
worldAabb.m_minVec = aabbAMinOut;
|
||||
worldAabb.m_minIndices[3] = bodyId;
|
||||
worldAabb.m_maxVec = aabbAMaxOut;
|
||||
worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
|
||||
worldAabbs[bodyId] = worldAabb;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_UPDATE_AABBS_H
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
project "Bullet3Collision"
|
||||
|
||||
language "C++"
|
||||
|
||||
kind "StaticLib"
|
||||
|
||||
includedirs {".."}
|
||||
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
|
||||
files {
|
||||
"**.cpp",
|
||||
"**.h"
|
||||
}
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
)
|
||||
|
||||
SET(Bullet3Common_SRCS
|
||||
b3AlignedAllocator.cpp
|
||||
b3Vector3.cpp
|
||||
b3Logging.cpp
|
||||
)
|
||||
|
||||
SET(Bullet3Common_HDRS
|
||||
b3AlignedAllocator.h
|
||||
b3AlignedObjectArray.h
|
||||
b3CommandLineArgs.h
|
||||
b3HashMap.h
|
||||
b3Logging.h
|
||||
b3Matrix3x3.h
|
||||
b3MinMax.h
|
||||
b3PoolAllocator.h
|
||||
b3QuadWord.h
|
||||
b3Quaternion.h
|
||||
b3Random.h
|
||||
b3Scalar.h
|
||||
b3StackAlloc.h
|
||||
b3Transform.h
|
||||
b3TransformUtil.h
|
||||
b3Vector3.h
|
||||
shared/b3Float4
|
||||
shared/b3Int2.h
|
||||
shared/b3Int4.h
|
||||
shared/b3Mat3x3.h
|
||||
shared/b3PlatformDefinitions
|
||||
shared/b3Quat.h
|
||||
)
|
||||
|
||||
ADD_LIBRARY(Bullet3Common ${Bullet3Common_SRCS} ${Bullet3Common_HDRS})
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#FILES_MATCHING requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Common DESTINATION .)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Common
|
||||
RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
|
||||
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES FRAMEWORK true)
|
||||
SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES PUBLIC_HEADER "${Bullet3Common_HDRS}")
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
||||
|
|
@ -1,186 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
#ifdef B3_ALLOCATOR_STATISTICS
|
||||
int b3g_numAlignedAllocs = 0;
|
||||
int b3g_numAlignedFree = 0;
|
||||
int b3g_totalBytesAlignedAllocs = 0; //detect memory leaks
|
||||
#endif
|
||||
|
||||
static void *b3AllocDefault(size_t size)
|
||||
{
|
||||
return malloc(size);
|
||||
}
|
||||
|
||||
static void b3FreeDefault(void *ptr)
|
||||
{
|
||||
free(ptr);
|
||||
}
|
||||
|
||||
static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
|
||||
static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
|
||||
|
||||
#if defined(B3_HAS_ALIGNED_ALLOCATOR)
|
||||
#include <malloc.h>
|
||||
static void *b3AlignedAllocDefault(size_t size, int alignment)
|
||||
{
|
||||
return _aligned_malloc(size, (size_t)alignment);
|
||||
}
|
||||
|
||||
static void b3AlignedFreeDefault(void *ptr)
|
||||
{
|
||||
_aligned_free(ptr);
|
||||
}
|
||||
#elif defined(__CELLOS_LV2__)
|
||||
#include <stdlib.h>
|
||||
|
||||
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
|
||||
{
|
||||
return memalign(alignment, size);
|
||||
}
|
||||
|
||||
static inline void b3AlignedFreeDefault(void *ptr)
|
||||
{
|
||||
free(ptr);
|
||||
}
|
||||
#else
|
||||
|
||||
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
|
||||
{
|
||||
void *ret;
|
||||
char *real;
|
||||
real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
|
||||
if (real)
|
||||
{
|
||||
ret = b3AlignPointer(real + sizeof(void *), alignment);
|
||||
*((void **)(ret)-1) = (void *)(real);
|
||||
}
|
||||
else
|
||||
{
|
||||
ret = (void *)(real);
|
||||
}
|
||||
return (ret);
|
||||
}
|
||||
|
||||
static inline void b3AlignedFreeDefault(void *ptr)
|
||||
{
|
||||
void *real;
|
||||
|
||||
if (ptr)
|
||||
{
|
||||
real = *((void **)(ptr)-1);
|
||||
b3s_freeFunc(real);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
|
||||
static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
|
||||
|
||||
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
|
||||
{
|
||||
b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
|
||||
b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
|
||||
}
|
||||
|
||||
void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
|
||||
{
|
||||
b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
|
||||
b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
|
||||
}
|
||||
|
||||
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
//this generic allocator provides the total allocated number of bytes
|
||||
#include <stdio.h>
|
||||
|
||||
void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filename)
|
||||
{
|
||||
void *ret;
|
||||
char *real;
|
||||
#ifdef B3_ALLOCATOR_STATISTICS
|
||||
b3g_totalBytesAlignedAllocs += size;
|
||||
b3g_numAlignedAllocs++;
|
||||
#endif
|
||||
real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
|
||||
if (real)
|
||||
{
|
||||
ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
|
||||
*((void **)(ret)-1) = (void *)(real);
|
||||
*((int *)(ret)-2) = size;
|
||||
}
|
||||
else
|
||||
{
|
||||
ret = (void *)(real); //??
|
||||
}
|
||||
|
||||
b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedAllocs, real, filename, line, size);
|
||||
|
||||
int *ptr = (int *)ret;
|
||||
*ptr = 12;
|
||||
return (ret);
|
||||
}
|
||||
|
||||
void b3AlignedFreeInternal(void *ptr, int line, char *filename)
|
||||
{
|
||||
void *real;
|
||||
#ifdef B3_ALLOCATOR_STATISTICS
|
||||
b3g_numAlignedFree++;
|
||||
#endif
|
||||
if (ptr)
|
||||
{
|
||||
real = *((void **)(ptr)-1);
|
||||
int size = *((int *)(ptr)-2);
|
||||
#ifdef B3_ALLOCATOR_STATISTICS
|
||||
b3g_totalBytesAlignedAllocs -= size;
|
||||
#endif
|
||||
b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
|
||||
|
||||
b3s_freeFunc(real);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Printf("NULL ptr\n");
|
||||
}
|
||||
}
|
||||
|
||||
#else //B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
|
||||
void *b3AlignedAllocInternal(size_t size, int alignment)
|
||||
{
|
||||
#ifdef B3_ALLOCATOR_STATISTICS
|
||||
b3g_numAlignedAllocs++;
|
||||
#endif
|
||||
void *ptr;
|
||||
ptr = b3s_alignedAllocFunc(size, alignment);
|
||||
// b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
void b3AlignedFreeInternal(void *ptr)
|
||||
{
|
||||
if (!ptr)
|
||||
{
|
||||
return;
|
||||
}
|
||||
#ifdef B3_ALLOCATOR_STATISTICS
|
||||
b3g_numAlignedFree++;
|
||||
#endif
|
||||
// b3Printf("b3AlignedFreeInternal %x\n",ptr);
|
||||
b3s_alignedFreeFunc(ptr);
|
||||
}
|
||||
|
||||
#endif //B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
|
|
@ -1,110 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_ALIGNED_ALLOCATOR
|
||||
#define B3_ALIGNED_ALLOCATOR
|
||||
|
||||
///we probably replace this with our own aligned memory allocator
|
||||
///so we replace _aligned_malloc and _aligned_free with our own
|
||||
///that is better portable and more predictable
|
||||
|
||||
#include "b3Scalar.h"
|
||||
//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
|
||||
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
|
||||
|
||||
#define b3AlignedAlloc(a, b) \
|
||||
b3AlignedAllocInternal(a, b, __LINE__, __FILE__)
|
||||
|
||||
#define b3AlignedFree(ptr) \
|
||||
b3AlignedFreeInternal(ptr, __LINE__, __FILE__)
|
||||
|
||||
void* b3AlignedAllocInternal(size_t size, int alignment, int line, char* filename);
|
||||
|
||||
void b3AlignedFreeInternal(void* ptr, int line, char* filename);
|
||||
|
||||
#else
|
||||
void* b3AlignedAllocInternal(size_t size, int alignment);
|
||||
void b3AlignedFreeInternal(void* ptr);
|
||||
|
||||
#define b3AlignedAlloc(size, alignment) b3AlignedAllocInternal(size, alignment)
|
||||
#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
|
||||
|
||||
#endif
|
||||
typedef int btSizeType;
|
||||
|
||||
typedef void*(b3AlignedAllocFunc)(size_t size, int alignment);
|
||||
typedef void(b3AlignedFreeFunc)(void* memblock);
|
||||
typedef void*(b3AllocFunc)(size_t size);
|
||||
typedef void(b3FreeFunc)(void* memblock);
|
||||
|
||||
///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
|
||||
void b3AlignedAllocSetCustom(b3AllocFunc* allocFunc, b3FreeFunc* freeFunc);
|
||||
///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
|
||||
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
|
||||
|
||||
///The b3AlignedAllocator is a portable class for aligned memory allocations.
|
||||
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
|
||||
template <typename T, unsigned Alignment>
|
||||
class b3AlignedAllocator
|
||||
{
|
||||
typedef b3AlignedAllocator<T, Alignment> self_type;
|
||||
|
||||
public:
|
||||
//just going down a list:
|
||||
b3AlignedAllocator() {}
|
||||
/*
|
||||
b3AlignedAllocator( const self_type & ) {}
|
||||
*/
|
||||
|
||||
template <typename Other>
|
||||
b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
|
||||
{
|
||||
}
|
||||
|
||||
typedef const T* const_pointer;
|
||||
typedef const T& const_reference;
|
||||
typedef T* pointer;
|
||||
typedef T& reference;
|
||||
typedef T value_type;
|
||||
|
||||
pointer address(reference ref) const { return &ref; }
|
||||
const_pointer address(const_reference ref) const { return &ref; }
|
||||
pointer allocate(btSizeType n, const_pointer* hint = 0)
|
||||
{
|
||||
(void)hint;
|
||||
return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
|
||||
}
|
||||
void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
|
||||
void deallocate(pointer ptr)
|
||||
{
|
||||
b3AlignedFree(reinterpret_cast<void*>(ptr));
|
||||
}
|
||||
void destroy(pointer ptr) { ptr->~value_type(); }
|
||||
|
||||
template <typename O>
|
||||
struct rebind
|
||||
{
|
||||
typedef b3AlignedAllocator<O, Alignment> other;
|
||||
};
|
||||
template <typename O>
|
||||
self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
|
||||
friend bool operator==(const self_type&, const self_type&) { return true; }
|
||||
};
|
||||
|
||||
#endif //B3_ALIGNED_ALLOCATOR
|
||||
|
|
@ -1,522 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_OBJECT_ARRAY__
|
||||
#define B3_OBJECT_ARRAY__
|
||||
|
||||
#include "b3Scalar.h" // has definitions like B3_FORCE_INLINE
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
|
||||
///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
|
||||
///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
|
||||
///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
|
||||
///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
|
||||
|
||||
#define B3_USE_PLACEMENT_NEW 1
|
||||
//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
|
||||
#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
|
||||
|
||||
#ifdef B3_USE_MEMCPY
|
||||
#include <memory.h>
|
||||
#include <string.h>
|
||||
#endif //B3_USE_MEMCPY
|
||||
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
#include <new> //for placement new
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
|
||||
///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
|
||||
///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
|
||||
template <typename T>
|
||||
//template <class T>
|
||||
class b3AlignedObjectArray
|
||||
{
|
||||
b3AlignedAllocator<T, 16> m_allocator;
|
||||
|
||||
int m_size;
|
||||
int m_capacity;
|
||||
T* m_data;
|
||||
//PCK: added this line
|
||||
bool m_ownsMemory;
|
||||
|
||||
#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
|
||||
public:
|
||||
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other)
|
||||
{
|
||||
copyFromArray(other);
|
||||
return *this;
|
||||
}
|
||||
#else //B3_ALLOW_ARRAY_COPY_OPERATOR
|
||||
private:
|
||||
B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other);
|
||||
#endif //B3_ALLOW_ARRAY_COPY_OPERATOR
|
||||
|
||||
protected:
|
||||
B3_FORCE_INLINE int allocSize(int size)
|
||||
{
|
||||
return (size ? size * 2 : 1);
|
||||
}
|
||||
B3_FORCE_INLINE void copy(int start, int end, T* dest) const
|
||||
{
|
||||
int i;
|
||||
for (i = start; i < end; ++i)
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
new (&dest[i]) T(m_data[i]);
|
||||
#else
|
||||
dest[i] = m_data[i];
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void init()
|
||||
{
|
||||
//PCK: added this line
|
||||
m_ownsMemory = true;
|
||||
m_data = 0;
|
||||
m_size = 0;
|
||||
m_capacity = 0;
|
||||
}
|
||||
B3_FORCE_INLINE void destroy(int first, int last)
|
||||
{
|
||||
int i;
|
||||
for (i = first; i < last; i++)
|
||||
{
|
||||
m_data[i].~T();
|
||||
}
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void* allocate(int size)
|
||||
{
|
||||
if (size)
|
||||
return m_allocator.allocate(size);
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void deallocate()
|
||||
{
|
||||
if (m_data)
|
||||
{
|
||||
//PCK: enclosed the deallocation in this block
|
||||
if (m_ownsMemory)
|
||||
{
|
||||
m_allocator.deallocate(m_data);
|
||||
}
|
||||
m_data = 0;
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
b3AlignedObjectArray()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
~b3AlignedObjectArray()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
|
||||
b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
|
||||
{
|
||||
init();
|
||||
|
||||
int otherSize = otherArray.size();
|
||||
resize(otherSize);
|
||||
otherArray.copy(0, otherSize, m_data);
|
||||
}
|
||||
|
||||
/// return the number of elements in the array
|
||||
B3_FORCE_INLINE int size() const
|
||||
{
|
||||
return m_size;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const T& at(int n) const
|
||||
{
|
||||
b3Assert(n >= 0);
|
||||
b3Assert(n < size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE T& at(int n)
|
||||
{
|
||||
b3Assert(n >= 0);
|
||||
b3Assert(n < size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const T& operator[](int n) const
|
||||
{
|
||||
b3Assert(n >= 0);
|
||||
b3Assert(n < size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE T& operator[](int n)
|
||||
{
|
||||
b3Assert(n >= 0);
|
||||
b3Assert(n < size());
|
||||
return m_data[n];
|
||||
}
|
||||
|
||||
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
|
||||
B3_FORCE_INLINE void clear()
|
||||
{
|
||||
destroy(0, size());
|
||||
|
||||
deallocate();
|
||||
|
||||
init();
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void pop_back()
|
||||
{
|
||||
b3Assert(m_size > 0);
|
||||
m_size--;
|
||||
m_data[m_size].~T();
|
||||
}
|
||||
|
||||
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
|
||||
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
|
||||
B3_FORCE_INLINE void resizeNoInitialize(int newsize)
|
||||
{
|
||||
int curSize = size();
|
||||
|
||||
if (newsize < curSize)
|
||||
{
|
||||
}
|
||||
else
|
||||
{
|
||||
if (newsize > size())
|
||||
{
|
||||
reserve(newsize);
|
||||
}
|
||||
//leave this uninitialized
|
||||
}
|
||||
m_size = newsize;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void resize(int newsize, const T& fillData = T())
|
||||
{
|
||||
int curSize = size();
|
||||
|
||||
if (newsize < curSize)
|
||||
{
|
||||
for (int i = newsize; i < curSize; i++)
|
||||
{
|
||||
m_data[i].~T();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (newsize > size())
|
||||
{
|
||||
reserve(newsize);
|
||||
}
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
for (int i = curSize; i < newsize; i++)
|
||||
{
|
||||
new (&m_data[i]) T(fillData);
|
||||
}
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
}
|
||||
|
||||
m_size = newsize;
|
||||
}
|
||||
B3_FORCE_INLINE T& expandNonInitializing()
|
||||
{
|
||||
int sz = size();
|
||||
if (sz == capacity())
|
||||
{
|
||||
reserve(allocSize(size()));
|
||||
}
|
||||
m_size++;
|
||||
|
||||
return m_data[sz];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE T& expand(const T& fillValue = T())
|
||||
{
|
||||
int sz = size();
|
||||
if (sz == capacity())
|
||||
{
|
||||
reserve(allocSize(size()));
|
||||
}
|
||||
m_size++;
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
|
||||
#endif
|
||||
|
||||
return m_data[sz];
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void push_back(const T& _Val)
|
||||
{
|
||||
int sz = size();
|
||||
if (sz == capacity())
|
||||
{
|
||||
reserve(allocSize(size()));
|
||||
}
|
||||
|
||||
#ifdef B3_USE_PLACEMENT_NEW
|
||||
new (&m_data[m_size]) T(_Val);
|
||||
#else
|
||||
m_data[size()] = _Val;
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
|
||||
m_size++;
|
||||
}
|
||||
|
||||
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
|
||||
B3_FORCE_INLINE int capacity() const
|
||||
{
|
||||
return m_capacity;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void reserve(int _Count)
|
||||
{ // determine new minimum length of allocated storage
|
||||
if (capacity() < _Count)
|
||||
{ // not enough room, reallocate
|
||||
T* s = (T*)allocate(_Count);
|
||||
b3Assert(s);
|
||||
if (s == 0)
|
||||
{
|
||||
b3Error("b3AlignedObjectArray reserve out-of-memory\n");
|
||||
_Count = 0;
|
||||
m_size = 0;
|
||||
}
|
||||
copy(0, size(), s);
|
||||
|
||||
destroy(0, size());
|
||||
|
||||
deallocate();
|
||||
|
||||
//PCK: added this line
|
||||
m_ownsMemory = true;
|
||||
|
||||
m_data = s;
|
||||
|
||||
m_capacity = _Count;
|
||||
}
|
||||
}
|
||||
|
||||
class less
|
||||
{
|
||||
public:
|
||||
bool operator()(const T& a, const T& b)
|
||||
{
|
||||
return (a < b);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename L>
|
||||
void quickSortInternal(const L& CompareFunc, int lo, int hi)
|
||||
{
|
||||
// lo is the lower index, hi is the upper index
|
||||
// of the region of array a that is to be sorted
|
||||
int i = lo, j = hi;
|
||||
T x = m_data[(lo + hi) / 2];
|
||||
|
||||
// partition
|
||||
do
|
||||
{
|
||||
while (CompareFunc(m_data[i], x))
|
||||
i++;
|
||||
while (CompareFunc(x, m_data[j]))
|
||||
j--;
|
||||
if (i <= j)
|
||||
{
|
||||
swap(i, j);
|
||||
i++;
|
||||
j--;
|
||||
}
|
||||
} while (i <= j);
|
||||
|
||||
// recursion
|
||||
if (lo < j)
|
||||
quickSortInternal(CompareFunc, lo, j);
|
||||
if (i < hi)
|
||||
quickSortInternal(CompareFunc, i, hi);
|
||||
}
|
||||
|
||||
template <typename L>
|
||||
void quickSort(const L& CompareFunc)
|
||||
{
|
||||
//don't sort 0 or 1 elements
|
||||
if (size() > 1)
|
||||
{
|
||||
quickSortInternal(CompareFunc, 0, size() - 1);
|
||||
}
|
||||
}
|
||||
|
||||
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
|
||||
template <typename L>
|
||||
void downHeap(T* pArr, int k, int n, const L& CompareFunc)
|
||||
{
|
||||
/* PRE: a[k+1..N] is a heap */
|
||||
/* POST: a[k..N] is a heap */
|
||||
|
||||
T temp = pArr[k - 1];
|
||||
/* k has child(s) */
|
||||
while (k <= n / 2)
|
||||
{
|
||||
int child = 2 * k;
|
||||
|
||||
if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
|
||||
{
|
||||
child++;
|
||||
}
|
||||
/* pick larger child */
|
||||
if (CompareFunc(temp, pArr[child - 1]))
|
||||
{
|
||||
/* move child up */
|
||||
pArr[k - 1] = pArr[child - 1];
|
||||
k = child;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
pArr[k - 1] = temp;
|
||||
} /*downHeap*/
|
||||
|
||||
void swap(int index0, int index1)
|
||||
{
|
||||
#ifdef B3_USE_MEMCPY
|
||||
char temp[sizeof(T)];
|
||||
memcpy(temp, &m_data[index0], sizeof(T));
|
||||
memcpy(&m_data[index0], &m_data[index1], sizeof(T));
|
||||
memcpy(&m_data[index1], temp, sizeof(T));
|
||||
#else
|
||||
T temp = m_data[index0];
|
||||
m_data[index0] = m_data[index1];
|
||||
m_data[index1] = temp;
|
||||
#endif //B3_USE_PLACEMENT_NEW
|
||||
}
|
||||
|
||||
template <typename L>
|
||||
void heapSort(const L& CompareFunc)
|
||||
{
|
||||
/* sort a[0..N-1], N.B. 0 to N-1 */
|
||||
int k;
|
||||
int n = m_size;
|
||||
for (k = n / 2; k > 0; k--)
|
||||
{
|
||||
downHeap(m_data, k, n, CompareFunc);
|
||||
}
|
||||
|
||||
/* a[1..N] is now a heap */
|
||||
while (n >= 1)
|
||||
{
|
||||
swap(0, n - 1); /* largest of a[0..n-1] */
|
||||
|
||||
n = n - 1;
|
||||
/* restore a[1..i-1] heap */
|
||||
downHeap(m_data, 1, n, CompareFunc);
|
||||
}
|
||||
}
|
||||
|
||||
///non-recursive binary search, assumes sorted array
|
||||
int findBinarySearch(const T& key) const
|
||||
{
|
||||
int first = 0;
|
||||
int last = size() - 1;
|
||||
|
||||
//assume sorted array
|
||||
while (first <= last)
|
||||
{
|
||||
int mid = (first + last) / 2; // compute mid point.
|
||||
if (key > m_data[mid])
|
||||
first = mid + 1; // repeat search in top half.
|
||||
else if (key < m_data[mid])
|
||||
last = mid - 1; // repeat search in bottom half.
|
||||
else
|
||||
return mid; // found it. return position /////
|
||||
}
|
||||
return size(); // failed to find key
|
||||
}
|
||||
|
||||
int findLinearSearch(const T& key) const
|
||||
{
|
||||
int index = size();
|
||||
int i;
|
||||
|
||||
for (i = 0; i < size(); i++)
|
||||
{
|
||||
if (m_data[i] == key)
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
int findLinearSearch2(const T& key) const
|
||||
{
|
||||
int index = -1;
|
||||
int i;
|
||||
|
||||
for (i = 0; i < size(); i++)
|
||||
{
|
||||
if (m_data[i] == key)
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
void remove(const T& key)
|
||||
{
|
||||
int findIndex = findLinearSearch(key);
|
||||
if (findIndex < size())
|
||||
{
|
||||
swap(findIndex, size() - 1);
|
||||
pop_back();
|
||||
}
|
||||
}
|
||||
|
||||
//PCK: whole function
|
||||
void initializeFromBuffer(void* buffer, int size, int capacity)
|
||||
{
|
||||
clear();
|
||||
m_ownsMemory = false;
|
||||
m_data = (T*)buffer;
|
||||
m_size = size;
|
||||
m_capacity = capacity;
|
||||
}
|
||||
|
||||
void copyFromArray(const b3AlignedObjectArray& otherArray)
|
||||
{
|
||||
int otherSize = otherArray.size();
|
||||
resize(otherSize);
|
||||
otherArray.copy(0, otherSize, m_data);
|
||||
}
|
||||
|
||||
void removeAtIndex(int index)
|
||||
{
|
||||
if (index < size())
|
||||
{
|
||||
swap(index, size() - 1);
|
||||
pop_back();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_OBJECT_ARRAY__
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
#ifndef COMMAND_LINE_ARGS_H
|
||||
#define COMMAND_LINE_ARGS_H
|
||||
|
||||
/******************************************************************************
|
||||
* Command-line parsing
|
||||
******************************************************************************/
|
||||
#include <map>
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <cstring>
|
||||
#include <sstream>
|
||||
class b3CommandLineArgs
|
||||
{
|
||||
protected:
|
||||
std::map<std::string, std::string> pairs;
|
||||
|
||||
public:
|
||||
// Constructor
|
||||
b3CommandLineArgs(int argc, char **argv)
|
||||
{
|
||||
addArgs(argc, argv);
|
||||
}
|
||||
|
||||
void addArgs(int argc, char **argv)
|
||||
{
|
||||
for (int i = 1; i < argc; i++)
|
||||
{
|
||||
std::string arg = argv[i];
|
||||
|
||||
if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
std::string::size_type pos;
|
||||
std::string key, val;
|
||||
if ((pos = arg.find('=')) == std::string::npos)
|
||||
{
|
||||
key = std::string(arg, 2, arg.length() - 2);
|
||||
val = "";
|
||||
}
|
||||
else
|
||||
{
|
||||
key = std::string(arg, 2, pos - 2);
|
||||
val = std::string(arg, pos + 1, arg.length() - 1);
|
||||
}
|
||||
|
||||
//only add new keys, don't replace existing
|
||||
if (pairs.find(key) == pairs.end())
|
||||
{
|
||||
pairs[key] = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool CheckCmdLineFlag(const char *arg_name)
|
||||
{
|
||||
std::map<std::string, std::string>::iterator itr;
|
||||
if ((itr = pairs.find(arg_name)) != pairs.end())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool GetCmdLineArgument(const char *arg_name, T &val);
|
||||
|
||||
int ParsedArgc()
|
||||
{
|
||||
return pairs.size();
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
|
||||
{
|
||||
std::map<std::string, std::string>::iterator itr;
|
||||
if ((itr = pairs.find(arg_name)) != pairs.end())
|
||||
{
|
||||
std::istringstream strstream(itr->second);
|
||||
strstream >> val;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
|
||||
{
|
||||
std::map<std::string, std::string>::iterator itr;
|
||||
if ((itr = pairs.find(arg_name)) != pairs.end())
|
||||
{
|
||||
std::string s = itr->second;
|
||||
val = (char *)malloc(sizeof(char) * (s.length() + 1));
|
||||
std::strcpy(val, s.c_str());
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
val = NULL;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif //COMMAND_LINE_ARGS_H
|
||||
|
|
@ -1,133 +0,0 @@
|
|||
#ifndef B3_FILE_UTILS_H
|
||||
#define B3_FILE_UTILS_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include "b3Scalar.h"
|
||||
#include <stddef.h> //ptrdiff_h
|
||||
#include <string.h>
|
||||
|
||||
struct b3FileUtils
|
||||
{
|
||||
b3FileUtils()
|
||||
{
|
||||
}
|
||||
virtual ~b3FileUtils()
|
||||
{
|
||||
}
|
||||
|
||||
static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
|
||||
{
|
||||
FILE* f = 0;
|
||||
f = fopen(orgFileName, "rb");
|
||||
if (f)
|
||||
{
|
||||
//printf("original file found: [%s]\n", orgFileName);
|
||||
sprintf(relativeFileName, "%s", orgFileName);
|
||||
fclose(f);
|
||||
return true;
|
||||
}
|
||||
|
||||
//printf("Trying various directories, relative to current working directory\n");
|
||||
const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
|
||||
int numPrefixes = sizeof(prefix) / sizeof(const char*);
|
||||
|
||||
f = 0;
|
||||
bool fileFound = false;
|
||||
|
||||
for (int i = 0; !f && i < numPrefixes; i++)
|
||||
{
|
||||
#ifdef _MSC_VER
|
||||
sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
|
||||
#else
|
||||
sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
|
||||
#endif
|
||||
f = fopen(relativeFileName, "rb");
|
||||
if (f)
|
||||
{
|
||||
fileFound = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
return fileFound;
|
||||
}
|
||||
|
||||
static const char* strip2(const char* name, const char* pattern)
|
||||
{
|
||||
size_t const patlen = strlen(pattern);
|
||||
size_t patcnt = 0;
|
||||
const char* oriptr;
|
||||
const char* patloc;
|
||||
// find how many times the pattern occurs in the original string
|
||||
for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
|
||||
{
|
||||
patcnt++;
|
||||
}
|
||||
return oriptr;
|
||||
}
|
||||
|
||||
static int extractPath(const char* fileName, char* path, int maxPathLength)
|
||||
{
|
||||
const char* stripped = strip2(fileName, "/");
|
||||
stripped = strip2(stripped, "\\");
|
||||
|
||||
ptrdiff_t len = stripped - fileName;
|
||||
b3Assert((len + 1) < maxPathLength);
|
||||
|
||||
if (len && ((len + 1) < maxPathLength))
|
||||
{
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
path[i] = fileName[i];
|
||||
}
|
||||
path[len] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
len = 0;
|
||||
b3Assert(maxPathLength > 0);
|
||||
if (maxPathLength > 0)
|
||||
{
|
||||
path[len] = 0;
|
||||
}
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
static char toLowerChar(const char t)
|
||||
{
|
||||
if (t >= (char)'A' && t <= (char)'Z')
|
||||
return t + ((char)'a' - (char)'A');
|
||||
else
|
||||
return t;
|
||||
}
|
||||
|
||||
static void toLower(char* str)
|
||||
{
|
||||
int len = strlen(str);
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
str[i] = toLowerChar(str[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/*static const char* strip2(const char* name, const char* pattern)
|
||||
{
|
||||
size_t const patlen = strlen(pattern);
|
||||
size_t patcnt = 0;
|
||||
const char * oriptr;
|
||||
const char * patloc;
|
||||
// find how many times the pattern occurs in the original string
|
||||
for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
|
||||
{
|
||||
patcnt++;
|
||||
}
|
||||
return oriptr;
|
||||
}
|
||||
*/
|
||||
};
|
||||
#endif //B3_FILE_UTILS_H
|
||||
|
|
@ -1,462 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_HASH_MAP_H
|
||||
#define B3_HASH_MAP_H
|
||||
|
||||
#include "b3AlignedObjectArray.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
///very basic hashable string implementation, compatible with b3HashMap
|
||||
struct b3HashString
|
||||
{
|
||||
std::string m_string;
|
||||
unsigned int m_hash;
|
||||
|
||||
B3_FORCE_INLINE unsigned int getHash() const
|
||||
{
|
||||
return m_hash;
|
||||
}
|
||||
|
||||
b3HashString(const char* name)
|
||||
: m_string(name)
|
||||
{
|
||||
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
|
||||
static const unsigned int InitialFNV = 2166136261u;
|
||||
static const unsigned int FNVMultiple = 16777619u;
|
||||
|
||||
/* Fowler / Noll / Vo (FNV) Hash */
|
||||
unsigned int hash = InitialFNV;
|
||||
int len = m_string.length();
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
hash = hash ^ (m_string[i]); /* xor the low 8 bits */
|
||||
hash = hash * FNVMultiple; /* multiply by the magic number */
|
||||
}
|
||||
m_hash = hash;
|
||||
}
|
||||
|
||||
int portableStringCompare(const char* src, const char* dst) const
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
while (!(ret = *(unsigned char*)src - *(unsigned char*)dst) && *dst)
|
||||
++src, ++dst;
|
||||
|
||||
if (ret < 0)
|
||||
ret = -1;
|
||||
else if (ret > 0)
|
||||
ret = 1;
|
||||
|
||||
return (ret);
|
||||
}
|
||||
|
||||
bool equals(const b3HashString& other) const
|
||||
{
|
||||
return (m_string == other.m_string);
|
||||
}
|
||||
};
|
||||
|
||||
const int B3_HASH_NULL = 0xffffffff;
|
||||
|
||||
class b3HashInt
|
||||
{
|
||||
int m_uid;
|
||||
|
||||
public:
|
||||
b3HashInt(int uid) : m_uid(uid)
|
||||
{
|
||||
}
|
||||
|
||||
int getUid1() const
|
||||
{
|
||||
return m_uid;
|
||||
}
|
||||
|
||||
void setUid1(int uid)
|
||||
{
|
||||
m_uid = uid;
|
||||
}
|
||||
|
||||
bool equals(const b3HashInt& other) const
|
||||
{
|
||||
return getUid1() == other.getUid1();
|
||||
}
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash() const
|
||||
{
|
||||
int key = m_uid;
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
};
|
||||
|
||||
class b3HashPtr
|
||||
{
|
||||
union {
|
||||
const void* m_pointer;
|
||||
int m_hashValues[2];
|
||||
};
|
||||
|
||||
public:
|
||||
b3HashPtr(const void* ptr)
|
||||
: m_pointer(ptr)
|
||||
{
|
||||
}
|
||||
|
||||
const void* getPointer() const
|
||||
{
|
||||
return m_pointer;
|
||||
}
|
||||
|
||||
bool equals(const b3HashPtr& other) const
|
||||
{
|
||||
return getPointer() == other.getPointer();
|
||||
}
|
||||
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash() const
|
||||
{
|
||||
const bool VOID_IS_8 = ((sizeof(void*) == 8));
|
||||
|
||||
int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
|
||||
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
};
|
||||
|
||||
template <class Value>
|
||||
class b3HashKeyPtr
|
||||
{
|
||||
int m_uid;
|
||||
|
||||
public:
|
||||
b3HashKeyPtr(int uid) : m_uid(uid)
|
||||
{
|
||||
}
|
||||
|
||||
int getUid1() const
|
||||
{
|
||||
return m_uid;
|
||||
}
|
||||
|
||||
bool equals(const b3HashKeyPtr<Value>& other) const
|
||||
{
|
||||
return getUid1() == other.getUid1();
|
||||
}
|
||||
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash() const
|
||||
{
|
||||
int key = m_uid;
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
};
|
||||
|
||||
template <class Value>
|
||||
class b3HashKey
|
||||
{
|
||||
int m_uid;
|
||||
|
||||
public:
|
||||
b3HashKey(int uid) : m_uid(uid)
|
||||
{
|
||||
}
|
||||
|
||||
int getUid1() const
|
||||
{
|
||||
return m_uid;
|
||||
}
|
||||
|
||||
bool equals(const b3HashKey<Value>& other) const
|
||||
{
|
||||
return getUid1() == other.getUid1();
|
||||
}
|
||||
//to our success
|
||||
B3_FORCE_INLINE unsigned int getHash() const
|
||||
{
|
||||
int key = m_uid;
|
||||
// Thomas Wang's hash
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
};
|
||||
|
||||
///The b3HashMap template class implements a generic and lightweight hashmap.
|
||||
///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
|
||||
template <class Key, class Value>
|
||||
class b3HashMap
|
||||
{
|
||||
protected:
|
||||
b3AlignedObjectArray<int> m_hashTable;
|
||||
b3AlignedObjectArray<int> m_next;
|
||||
|
||||
b3AlignedObjectArray<Value> m_valueArray;
|
||||
b3AlignedObjectArray<Key> m_keyArray;
|
||||
|
||||
void growTables(const Key& /*key*/)
|
||||
{
|
||||
int newCapacity = m_valueArray.capacity();
|
||||
|
||||
if (m_hashTable.size() < newCapacity)
|
||||
{
|
||||
//grow hashtable and next table
|
||||
int curHashtableSize = m_hashTable.size();
|
||||
|
||||
m_hashTable.resize(newCapacity);
|
||||
m_next.resize(newCapacity);
|
||||
|
||||
int i;
|
||||
|
||||
for (i = 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_hashTable[i] = B3_HASH_NULL;
|
||||
}
|
||||
for (i = 0; i < newCapacity; ++i)
|
||||
{
|
||||
m_next[i] = B3_HASH_NULL;
|
||||
}
|
||||
|
||||
for (i = 0; i < curHashtableSize; i++)
|
||||
{
|
||||
//const Value& value = m_valueArray[i];
|
||||
//const Key& key = m_keyArray[i];
|
||||
|
||||
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1); // New hash value with new mask
|
||||
m_next[i] = m_hashTable[hashValue];
|
||||
m_hashTable[hashValue] = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
void insert(const Key& key, const Value& value)
|
||||
{
|
||||
int hash = key.getHash() & (m_valueArray.capacity() - 1);
|
||||
|
||||
//replace value if the key is already there
|
||||
int index = findIndex(key);
|
||||
if (index != B3_HASH_NULL)
|
||||
{
|
||||
m_valueArray[index] = value;
|
||||
return;
|
||||
}
|
||||
|
||||
int count = m_valueArray.size();
|
||||
int oldCapacity = m_valueArray.capacity();
|
||||
m_valueArray.push_back(value);
|
||||
m_keyArray.push_back(key);
|
||||
|
||||
int newCapacity = m_valueArray.capacity();
|
||||
if (oldCapacity < newCapacity)
|
||||
{
|
||||
growTables(key);
|
||||
//hash with new capacity
|
||||
hash = key.getHash() & (m_valueArray.capacity() - 1);
|
||||
}
|
||||
m_next[count] = m_hashTable[hash];
|
||||
m_hashTable[hash] = count;
|
||||
}
|
||||
|
||||
void remove(const Key& key)
|
||||
{
|
||||
int hash = key.getHash() & (m_valueArray.capacity() - 1);
|
||||
|
||||
int pairIndex = findIndex(key);
|
||||
|
||||
if (pairIndex == B3_HASH_NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Remove the pair from the hash table.
|
||||
int index = m_hashTable[hash];
|
||||
b3Assert(index != B3_HASH_NULL);
|
||||
|
||||
int previous = B3_HASH_NULL;
|
||||
while (index != pairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_HASH_NULL)
|
||||
{
|
||||
b3Assert(m_next[previous] == pairIndex);
|
||||
m_next[previous] = m_next[pairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[hash] = m_next[pairIndex];
|
||||
}
|
||||
|
||||
// We now move the last pair into spot of the
|
||||
// pair being removed. We need to fix the hash
|
||||
// table indices to support the move.
|
||||
|
||||
int lastPairIndex = m_valueArray.size() - 1;
|
||||
|
||||
// If the removed pair is the last pair, we are done.
|
||||
if (lastPairIndex == pairIndex)
|
||||
{
|
||||
m_valueArray.pop_back();
|
||||
m_keyArray.pop_back();
|
||||
return;
|
||||
}
|
||||
|
||||
// Remove the last pair from the hash table.
|
||||
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
|
||||
|
||||
index = m_hashTable[lastHash];
|
||||
b3Assert(index != B3_HASH_NULL);
|
||||
|
||||
previous = B3_HASH_NULL;
|
||||
while (index != lastPairIndex)
|
||||
{
|
||||
previous = index;
|
||||
index = m_next[index];
|
||||
}
|
||||
|
||||
if (previous != B3_HASH_NULL)
|
||||
{
|
||||
b3Assert(m_next[previous] == lastPairIndex);
|
||||
m_next[previous] = m_next[lastPairIndex];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_hashTable[lastHash] = m_next[lastPairIndex];
|
||||
}
|
||||
|
||||
// Copy the last pair into the remove pair's spot.
|
||||
m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
|
||||
m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
|
||||
|
||||
// Insert the last pair into the hash table
|
||||
m_next[pairIndex] = m_hashTable[lastHash];
|
||||
m_hashTable[lastHash] = pairIndex;
|
||||
|
||||
m_valueArray.pop_back();
|
||||
m_keyArray.pop_back();
|
||||
}
|
||||
|
||||
int size() const
|
||||
{
|
||||
return m_valueArray.size();
|
||||
}
|
||||
|
||||
const Value* getAtIndex(int index) const
|
||||
{
|
||||
b3Assert(index < m_valueArray.size());
|
||||
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
Value* getAtIndex(int index)
|
||||
{
|
||||
b3Assert(index < m_valueArray.size());
|
||||
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
Key getKeyAtIndex(int index)
|
||||
{
|
||||
b3Assert(index < m_keyArray.size());
|
||||
return m_keyArray[index];
|
||||
}
|
||||
|
||||
const Key getKeyAtIndex(int index) const
|
||||
{
|
||||
b3Assert(index < m_keyArray.size());
|
||||
return m_keyArray[index];
|
||||
}
|
||||
|
||||
Value* operator[](const Key& key)
|
||||
{
|
||||
return find(key);
|
||||
}
|
||||
|
||||
const Value* find(const Key& key) const
|
||||
{
|
||||
int index = findIndex(key);
|
||||
if (index == B3_HASH_NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
Value* find(const Key& key)
|
||||
{
|
||||
int index = findIndex(key);
|
||||
if (index == B3_HASH_NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return &m_valueArray[index];
|
||||
}
|
||||
|
||||
int findIndex(const Key& key) const
|
||||
{
|
||||
unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
|
||||
|
||||
if (hash >= (unsigned int)m_hashTable.size())
|
||||
{
|
||||
return B3_HASH_NULL;
|
||||
}
|
||||
|
||||
int index = m_hashTable[hash];
|
||||
while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
|
||||
{
|
||||
index = m_next[index];
|
||||
}
|
||||
return index;
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
m_hashTable.clear();
|
||||
m_next.clear();
|
||||
m_valueArray.clear();
|
||||
m_keyArray.clear();
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_HASH_MAP_H
|
||||
|
|
@ -1,145 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2013 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#include "b3Logging.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#endif //_WIN32
|
||||
|
||||
void b3PrintfFuncDefault(const char* msg)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
OutputDebugStringA(msg);
|
||||
#endif
|
||||
printf("%s", msg);
|
||||
//is this portable?
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
void b3WarningMessageFuncDefault(const char* msg)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
OutputDebugStringA(msg);
|
||||
#endif
|
||||
printf("%s", msg);
|
||||
//is this portable?
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
void b3ErrorMessageFuncDefault(const char* msg)
|
||||
{
|
||||
#ifdef _WIN32
|
||||
OutputDebugStringA(msg);
|
||||
#endif
|
||||
printf("%s", msg);
|
||||
|
||||
//is this portable?
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
|
||||
static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
|
||||
static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
|
||||
|
||||
///The developer can route b3Printf output using their own implementation
|
||||
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
|
||||
{
|
||||
b3s_printfFunc = printfFunc;
|
||||
}
|
||||
void b3SetCustomWarningMessageFunc(b3PrintfFunc* warningMessageFunc)
|
||||
{
|
||||
b3s_warningMessageFunc = warningMessageFunc;
|
||||
}
|
||||
void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
|
||||
{
|
||||
b3s_errorMessageFunc = errorMessageFunc;
|
||||
}
|
||||
|
||||
//#define B3_MAX_DEBUG_STRING_LENGTH 2048
|
||||
#define B3_MAX_DEBUG_STRING_LENGTH 32768
|
||||
|
||||
void b3OutputPrintfVarArgsInternal(const char* str, ...)
|
||||
{
|
||||
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
|
||||
va_list argList;
|
||||
va_start(argList, str);
|
||||
#ifdef _MSC_VER
|
||||
vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
|
||||
#else
|
||||
vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
|
||||
#endif
|
||||
(b3s_printfFunc)(strDebug);
|
||||
va_end(argList);
|
||||
}
|
||||
void b3OutputWarningMessageVarArgsInternal(const char* str, ...)
|
||||
{
|
||||
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
|
||||
va_list argList;
|
||||
va_start(argList, str);
|
||||
#ifdef _MSC_VER
|
||||
vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
|
||||
#else
|
||||
vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
|
||||
#endif
|
||||
(b3s_warningMessageFunc)(strDebug);
|
||||
va_end(argList);
|
||||
}
|
||||
void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
|
||||
{
|
||||
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
|
||||
va_list argList;
|
||||
va_start(argList, str);
|
||||
#ifdef _MSC_VER
|
||||
vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
|
||||
#else
|
||||
vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
|
||||
#endif
|
||||
(b3s_errorMessageFunc)(strDebug);
|
||||
va_end(argList);
|
||||
}
|
||||
|
||||
void b3EnterProfileZoneDefault(const char* name)
|
||||
{
|
||||
}
|
||||
void b3LeaveProfileZoneDefault()
|
||||
{
|
||||
}
|
||||
static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
|
||||
static b3LeaveProfileZoneFunc* b3s_leaveFunc = b3LeaveProfileZoneDefault;
|
||||
void b3EnterProfileZone(const char* name)
|
||||
{
|
||||
(b3s_enterFunc)(name);
|
||||
}
|
||||
void b3LeaveProfileZone()
|
||||
{
|
||||
(b3s_leaveFunc)();
|
||||
}
|
||||
|
||||
void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc)
|
||||
{
|
||||
b3s_enterFunc = enterFunc;
|
||||
}
|
||||
void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
|
||||
{
|
||||
b3s_leaveFunc = leaveFunc;
|
||||
}
|
||||
|
||||
#ifndef _MSC_VER
|
||||
#undef vsprintf_s
|
||||
#endif
|
||||
|
|
@ -1,86 +0,0 @@
|
|||
|
||||
#ifndef B3_LOGGING_H
|
||||
#define B3_LOGGING_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
|
||||
///You can also customize the message by uncommenting out a different line below
|
||||
#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
|
||||
//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
|
||||
//#define b3Printf b3OutputPrintfVarArgsInternal
|
||||
//#define b3Printf(...) printf(__VA_ARGS__)
|
||||
//#define b3Printf(...)
|
||||
|
||||
#define b3Warning(...) \
|
||||
do \
|
||||
{ \
|
||||
b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__); \
|
||||
b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); \
|
||||
} while (0)
|
||||
#define b3Error(...) \
|
||||
do \
|
||||
{ \
|
||||
b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__); \
|
||||
b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); \
|
||||
} while (0)
|
||||
|
||||
#ifndef B3_NO_PROFILE
|
||||
|
||||
void b3EnterProfileZone(const char* name);
|
||||
void b3LeaveProfileZone();
|
||||
#ifdef __cplusplus
|
||||
|
||||
class b3ProfileZone
|
||||
{
|
||||
public:
|
||||
b3ProfileZone(const char* name)
|
||||
{
|
||||
b3EnterProfileZone(name);
|
||||
}
|
||||
|
||||
~b3ProfileZone()
|
||||
{
|
||||
b3LeaveProfileZone();
|
||||
}
|
||||
};
|
||||
|
||||
#define B3_PROFILE(name) b3ProfileZone __profile(name)
|
||||
#endif
|
||||
|
||||
#else //B3_NO_PROFILE
|
||||
|
||||
#define B3_PROFILE(name)
|
||||
#define b3StartProfile(a)
|
||||
#define b3StopProfile
|
||||
|
||||
#endif //#ifndef B3_NO_PROFILE
|
||||
|
||||
typedef void(b3PrintfFunc)(const char* msg);
|
||||
typedef void(b3WarningMessageFunc)(const char* msg);
|
||||
typedef void(b3ErrorMessageFunc)(const char* msg);
|
||||
typedef void(b3EnterProfileZoneFunc)(const char* msg);
|
||||
typedef void(b3LeaveProfileZoneFunc)();
|
||||
|
||||
///The developer can route b3Printf output using their own implementation
|
||||
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
|
||||
void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
|
||||
void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
|
||||
|
||||
///Set custom profile zone functions (zones can be nested)
|
||||
void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
|
||||
void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
|
||||
|
||||
///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
|
||||
void b3OutputPrintfVarArgsInternal(const char* str, ...);
|
||||
void b3OutputWarningMessageVarArgsInternal(const char* str, ...);
|
||||
void b3OutputErrorMessageVarArgsInternal(const char* str, ...);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //B3_LOGGING_H
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,69 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_GEN_MINMAX_H
|
||||
#define B3_GEN_MINMAX_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
|
||||
{
|
||||
return a < b ? a : b;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
|
||||
{
|
||||
return a > b ? a : b;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
|
||||
{
|
||||
return a < lb ? lb : (ub < a ? ub : a);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
|
||||
{
|
||||
if (b < a)
|
||||
{
|
||||
a = b;
|
||||
}
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
|
||||
{
|
||||
if (a < b)
|
||||
{
|
||||
a = b;
|
||||
}
|
||||
}
|
||||
|
||||
template <class T>
|
||||
B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
|
||||
{
|
||||
if (a < lb)
|
||||
{
|
||||
a = lb;
|
||||
}
|
||||
else if (ub < a)
|
||||
{
|
||||
a = ub;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //B3_GEN_MINMAX_H
|
||||
|
|
@ -1,121 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef _BT_POOL_ALLOCATOR_H
|
||||
#define _BT_POOL_ALLOCATOR_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
|
||||
class b3PoolAllocator
|
||||
{
|
||||
int m_elemSize;
|
||||
int m_maxElements;
|
||||
int m_freeCount;
|
||||
void* m_firstFree;
|
||||
unsigned char* m_pool;
|
||||
|
||||
public:
|
||||
b3PoolAllocator(int elemSize, int maxElements)
|
||||
: m_elemSize(elemSize),
|
||||
m_maxElements(maxElements)
|
||||
{
|
||||
m_pool = (unsigned char*)b3AlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
|
||||
|
||||
unsigned char* p = m_pool;
|
||||
m_firstFree = p;
|
||||
m_freeCount = m_maxElements;
|
||||
int count = m_maxElements;
|
||||
while (--count)
|
||||
{
|
||||
*(void**)p = (p + m_elemSize);
|
||||
p += m_elemSize;
|
||||
}
|
||||
*(void**)p = 0;
|
||||
}
|
||||
|
||||
~b3PoolAllocator()
|
||||
{
|
||||
b3AlignedFree(m_pool);
|
||||
}
|
||||
|
||||
int getFreeCount() const
|
||||
{
|
||||
return m_freeCount;
|
||||
}
|
||||
|
||||
int getUsedCount() const
|
||||
{
|
||||
return m_maxElements - m_freeCount;
|
||||
}
|
||||
|
||||
int getMaxCount() const
|
||||
{
|
||||
return m_maxElements;
|
||||
}
|
||||
|
||||
void* allocate(int size)
|
||||
{
|
||||
// release mode fix
|
||||
(void)size;
|
||||
b3Assert(!size || size <= m_elemSize);
|
||||
b3Assert(m_freeCount > 0);
|
||||
void* result = m_firstFree;
|
||||
m_firstFree = *(void**)m_firstFree;
|
||||
--m_freeCount;
|
||||
return result;
|
||||
}
|
||||
|
||||
bool validPtr(void* ptr)
|
||||
{
|
||||
if (ptr)
|
||||
{
|
||||
if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void freeMemory(void* ptr)
|
||||
{
|
||||
if (ptr)
|
||||
{
|
||||
b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
|
||||
|
||||
*(void**)ptr = m_firstFree;
|
||||
m_firstFree = ptr;
|
||||
++m_freeCount;
|
||||
}
|
||||
}
|
||||
|
||||
int getElementSize() const
|
||||
{
|
||||
return m_elemSize;
|
||||
}
|
||||
|
||||
unsigned char* getPoolAddress()
|
||||
{
|
||||
return m_pool;
|
||||
}
|
||||
|
||||
const unsigned char* getPoolAddress() const
|
||||
{
|
||||
return m_pool;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //_BT_POOL_ALLOCATOR_H
|
||||
|
|
@ -1,242 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_SIMD_QUADWORD_H
|
||||
#define B3_SIMD_QUADWORD_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
#include "b3MinMax.h"
|
||||
|
||||
#if defined(__CELLOS_LV2) && defined(__SPU__)
|
||||
#include <altivec.h>
|
||||
#endif
|
||||
|
||||
/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion.
|
||||
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
|
||||
*/
|
||||
#ifndef USE_LIBSPE2
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3QuadWord
|
||||
#else
|
||||
class b3QuadWord
|
||||
#endif
|
||||
{
|
||||
protected:
|
||||
#if defined(__SPU__) && defined(__CELLOS_LV2__)
|
||||
union {
|
||||
vec_float4 mVec128;
|
||||
b3Scalar m_floats[4];
|
||||
};
|
||||
|
||||
public:
|
||||
vec_float4 get128() const
|
||||
{
|
||||
return mVec128;
|
||||
}
|
||||
|
||||
#else //__CELLOS_LV2__ __SPU__
|
||||
|
||||
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
|
||||
public:
|
||||
union {
|
||||
b3SimdFloat4 mVec128;
|
||||
b3Scalar m_floats[4];
|
||||
struct
|
||||
{
|
||||
b3Scalar x, y, z, w;
|
||||
};
|
||||
};
|
||||
|
||||
public:
|
||||
B3_FORCE_INLINE b3SimdFloat4 get128() const
|
||||
{
|
||||
return mVec128;
|
||||
}
|
||||
B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
|
||||
{
|
||||
mVec128 = v128;
|
||||
}
|
||||
#else
|
||||
public:
|
||||
union {
|
||||
b3Scalar m_floats[4];
|
||||
struct
|
||||
{
|
||||
b3Scalar x, y, z, w;
|
||||
};
|
||||
};
|
||||
#endif // B3_USE_SSE
|
||||
|
||||
#endif //__CELLOS_LV2__ __SPU__
|
||||
|
||||
public:
|
||||
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
|
||||
|
||||
// Set Vector
|
||||
B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
|
||||
{
|
||||
mVec128 = vec;
|
||||
}
|
||||
|
||||
// Copy constructor
|
||||
B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
|
||||
{
|
||||
mVec128 = rhs.mVec128;
|
||||
}
|
||||
|
||||
// Assignment Operator
|
||||
B3_FORCE_INLINE b3QuadWord&
|
||||
operator=(const b3QuadWord& v)
|
||||
{
|
||||
mVec128 = v.mVec128;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/**@brief Return the x value */
|
||||
B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
|
||||
/**@brief Return the y value */
|
||||
B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
|
||||
/**@brief Return the z value */
|
||||
B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
|
||||
/**@brief Set the x value */
|
||||
B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
|
||||
/**@brief Set the y value */
|
||||
B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
|
||||
/**@brief Set the z value */
|
||||
B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
|
||||
/**@brief Set the w value */
|
||||
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
|
||||
/**@brief Return the x value */
|
||||
|
||||
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
|
||||
//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
|
||||
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
|
||||
B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
|
||||
B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
|
||||
|
||||
B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
|
||||
{
|
||||
#ifdef B3_USE_SSE
|
||||
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
|
||||
#else
|
||||
return ((m_floats[3] == other.m_floats[3]) &&
|
||||
(m_floats[2] == other.m_floats[2]) &&
|
||||
(m_floats[1] == other.m_floats[1]) &&
|
||||
(m_floats[0] == other.m_floats[0]));
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
|
||||
{
|
||||
return !(*this == other);
|
||||
}
|
||||
|
||||
/**@brief Set x,y,z and zero w
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
*/
|
||||
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
|
||||
{
|
||||
m_floats[0] = _x;
|
||||
m_floats[1] = _y;
|
||||
m_floats[2] = _z;
|
||||
m_floats[3] = 0.f;
|
||||
}
|
||||
|
||||
/* void getValue(b3Scalar *m) const
|
||||
{
|
||||
m[0] = m_floats[0];
|
||||
m[1] = m_floats[1];
|
||||
m[2] = m_floats[2];
|
||||
}
|
||||
*/
|
||||
/**@brief Set the values
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
* @param w Value of w
|
||||
*/
|
||||
B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
|
||||
{
|
||||
m_floats[0] = _x;
|
||||
m_floats[1] = _y;
|
||||
m_floats[2] = _z;
|
||||
m_floats[3] = _w;
|
||||
}
|
||||
/**@brief No initialization constructor */
|
||||
B3_FORCE_INLINE b3QuadWord()
|
||||
// :m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
|
||||
{
|
||||
}
|
||||
|
||||
/**@brief Three argument constructor (zeros w)
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
*/
|
||||
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
|
||||
{
|
||||
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
|
||||
}
|
||||
|
||||
/**@brief Initializing constructor
|
||||
* @param x Value of x
|
||||
* @param y Value of y
|
||||
* @param z Value of z
|
||||
* @param w Value of w
|
||||
*/
|
||||
B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
|
||||
{
|
||||
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
|
||||
}
|
||||
|
||||
/**@brief Set each element to the max of the current values and the values of another b3QuadWord
|
||||
* @param other The other b3QuadWord to compare with
|
||||
*/
|
||||
B3_FORCE_INLINE void setMax(const b3QuadWord& other)
|
||||
{
|
||||
#ifdef B3_USE_SSE
|
||||
mVec128 = _mm_max_ps(mVec128, other.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vmaxq_f32(mVec128, other.mVec128);
|
||||
#else
|
||||
b3SetMax(m_floats[0], other.m_floats[0]);
|
||||
b3SetMax(m_floats[1], other.m_floats[1]);
|
||||
b3SetMax(m_floats[2], other.m_floats[2]);
|
||||
b3SetMax(m_floats[3], other.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
/**@brief Set each element to the min of the current values and the values of another b3QuadWord
|
||||
* @param other The other b3QuadWord to compare with
|
||||
*/
|
||||
B3_FORCE_INLINE void setMin(const b3QuadWord& other)
|
||||
{
|
||||
#ifdef B3_USE_SSE
|
||||
mVec128 = _mm_min_ps(mVec128, other.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vminq_f32(mVec128, other.mVec128);
|
||||
#else
|
||||
b3SetMin(m_floats[0], other.m_floats[0]);
|
||||
b3SetMin(m_floats[1], other.m_floats[1]);
|
||||
b3SetMin(m_floats[2], other.m_floats[2]);
|
||||
b3SetMin(m_floats[3], other.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_SIMD_QUADWORD_H
|
||||
|
|
@ -1,905 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_SIMD__QUATERNION_H_
|
||||
#define B3_SIMD__QUATERNION_H_
|
||||
|
||||
#include "b3Vector3.h"
|
||||
#include "b3QuadWord.h"
|
||||
|
||||
#ifdef B3_USE_SSE
|
||||
|
||||
const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
|
||||
|
||||
const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
|
||||
const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
|
||||
|
||||
#endif
|
||||
|
||||
/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
|
||||
class b3Quaternion : public b3QuadWord
|
||||
{
|
||||
public:
|
||||
/**@brief No initialization constructor */
|
||||
b3Quaternion() {}
|
||||
|
||||
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
|
||||
// Set Vector
|
||||
B3_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec)
|
||||
{
|
||||
mVec128 = vec;
|
||||
}
|
||||
|
||||
// Copy constructor
|
||||
B3_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs)
|
||||
{
|
||||
mVec128 = rhs.mVec128;
|
||||
}
|
||||
|
||||
// Assignment Operator
|
||||
B3_FORCE_INLINE b3Quaternion&
|
||||
operator=(const b3Quaternion& v)
|
||||
{
|
||||
mVec128 = v.mVec128;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// template <typename b3Scalar>
|
||||
// explicit Quaternion(const b3Scalar *v) : Tuple4<b3Scalar>(v) {}
|
||||
/**@brief Constructor from scalars */
|
||||
b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
|
||||
: b3QuadWord(_x, _y, _z, _w)
|
||||
{
|
||||
//b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
|
||||
}
|
||||
/**@brief Axis angle Constructor
|
||||
* @param axis The axis which the rotation is around
|
||||
* @param angle The magnitude of the rotation around the angle (Radians) */
|
||||
b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle)
|
||||
{
|
||||
setRotation(_axis, _angle);
|
||||
}
|
||||
/**@brief Constructor from Euler angles
|
||||
* @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z
|
||||
* @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y
|
||||
* @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */
|
||||
b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
|
||||
{
|
||||
#ifndef B3_EULER_DEFAULT_ZYX
|
||||
setEuler(yaw, pitch, roll);
|
||||
#else
|
||||
setEulerZYX(yaw, pitch, roll);
|
||||
#endif
|
||||
}
|
||||
/**@brief Set the rotation using axis angle notation
|
||||
* @param axis The axis around which to rotate
|
||||
* @param angle The magnitude of the rotation in Radians */
|
||||
void setRotation(const b3Vector3& axis, const b3Scalar& _angle)
|
||||
{
|
||||
b3Scalar d = axis.length();
|
||||
b3Assert(d != b3Scalar(0.0));
|
||||
if (d < B3_EPSILON)
|
||||
{
|
||||
setValue(0, 0, 0, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
|
||||
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
|
||||
b3Cos(_angle * b3Scalar(0.5)));
|
||||
}
|
||||
}
|
||||
/**@brief Set the quaternion using Euler angles
|
||||
* @param yaw Angle around Y
|
||||
* @param pitch Angle around X
|
||||
* @param roll Angle around Z */
|
||||
void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
|
||||
{
|
||||
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
|
||||
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
|
||||
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
|
||||
b3Scalar cosYaw = b3Cos(halfYaw);
|
||||
b3Scalar sinYaw = b3Sin(halfYaw);
|
||||
b3Scalar cosPitch = b3Cos(halfPitch);
|
||||
b3Scalar sinPitch = b3Sin(halfPitch);
|
||||
b3Scalar cosRoll = b3Cos(halfRoll);
|
||||
b3Scalar sinRoll = b3Sin(halfRoll);
|
||||
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
|
||||
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
|
||||
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
|
||||
}
|
||||
|
||||
/**@brief Set the quaternion using euler angles
|
||||
* @param yaw Angle around Z
|
||||
* @param pitch Angle around Y
|
||||
* @param roll Angle around X */
|
||||
void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
|
||||
{
|
||||
b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
|
||||
b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
|
||||
b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
|
||||
b3Scalar cosYaw = b3Cos(halfYaw);
|
||||
b3Scalar sinYaw = b3Sin(halfYaw);
|
||||
b3Scalar cosPitch = b3Cos(halfPitch);
|
||||
b3Scalar sinPitch = b3Sin(halfPitch);
|
||||
b3Scalar cosRoll = b3Cos(halfRoll);
|
||||
b3Scalar sinRoll = b3Sin(halfRoll);
|
||||
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
|
||||
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
|
||||
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
|
||||
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
|
||||
normalize();
|
||||
}
|
||||
|
||||
/**@brief Get the euler angles from this quaternion
|
||||
* @param yaw Angle around Z
|
||||
* @param pitch Angle around Y
|
||||
* @param roll Angle around X */
|
||||
void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
|
||||
{
|
||||
b3Scalar squ;
|
||||
b3Scalar sqx;
|
||||
b3Scalar sqy;
|
||||
b3Scalar sqz;
|
||||
b3Scalar sarg;
|
||||
sqx = m_floats[0] * m_floats[0];
|
||||
sqy = m_floats[1] * m_floats[1];
|
||||
sqz = m_floats[2] * m_floats[2];
|
||||
squ = m_floats[3] * m_floats[3];
|
||||
rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
|
||||
sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
|
||||
pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI : (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
|
||||
yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
|
||||
}
|
||||
|
||||
/**@brief Add two quaternions
|
||||
* @param q The quaternion to add to this one */
|
||||
B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
mVec128 = _mm_add_ps(mVec128, q.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vaddq_f32(mVec128, q.mVec128);
|
||||
#else
|
||||
m_floats[0] += q.getX();
|
||||
m_floats[1] += q.getY();
|
||||
m_floats[2] += q.getZ();
|
||||
m_floats[3] += q.m_floats[3];
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Subtract out a quaternion
|
||||
* @param q The quaternion to subtract from this one */
|
||||
b3Quaternion& operator-=(const b3Quaternion& q)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
mVec128 = _mm_sub_ps(mVec128, q.mVec128);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vsubq_f32(mVec128, q.mVec128);
|
||||
#else
|
||||
m_floats[0] -= q.getX();
|
||||
m_floats[1] -= q.getY();
|
||||
m_floats[2] -= q.getZ();
|
||||
m_floats[3] -= q.m_floats[3];
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Scale this quaternion
|
||||
* @param s The scalar to scale by */
|
||||
b3Quaternion& operator*=(const b3Scalar& s)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
|
||||
vs = b3_pshufd_ps(vs, 0); // (S S S S)
|
||||
mVec128 = _mm_mul_ps(mVec128, vs);
|
||||
#elif defined(B3_USE_NEON)
|
||||
mVec128 = vmulq_n_f32(mVec128, s);
|
||||
#else
|
||||
m_floats[0] *= s;
|
||||
m_floats[1] *= s;
|
||||
m_floats[2] *= s;
|
||||
m_floats[3] *= s;
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Multiply this quaternion by q on the right
|
||||
* @param q The other quaternion
|
||||
* Equivilant to this = this * q */
|
||||
b3Quaternion& operator*=(const b3Quaternion& q)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vQ2 = q.get128();
|
||||
|
||||
__m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0, 1, 2, 0));
|
||||
__m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
__m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 1));
|
||||
__m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2, 0, 1, 2));
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
|
||||
|
||||
B1 = B1 * B2; // A3 *= B3
|
||||
|
||||
mVec128 = b3_splat_ps(mVec128, 3); // A0
|
||||
mVec128 = mVec128 * vQ2; // A0 * B0
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
mVec128 = mVec128 - B1; // AB03 = AB0 - AB3
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
mVec128 = mVec128 + A1; // AB03 + AB12
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = mVec128;
|
||||
float32x4_t vQ2 = q.get128();
|
||||
float32x4_t A0, A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
|
||||
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
}
|
||||
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
|
||||
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
A0 = vaddq_f32(A0, A1); // AB03 + AB12
|
||||
|
||||
mVec128 = A0;
|
||||
#else
|
||||
setValue(
|
||||
m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(),
|
||||
m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(),
|
||||
m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(),
|
||||
m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ());
|
||||
#endif
|
||||
return *this;
|
||||
}
|
||||
/**@brief Return the dot product between this quaternion and another
|
||||
* @param q The other quaternion */
|
||||
b3Scalar dot(const b3Quaternion& q) const
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vd;
|
||||
|
||||
vd = _mm_mul_ps(mVec128, q.mVec128);
|
||||
|
||||
__m128 t = _mm_movehl_ps(vd, vd);
|
||||
vd = _mm_add_ps(vd, t);
|
||||
t = _mm_shuffle_ps(vd, vd, 0x55);
|
||||
vd = _mm_add_ss(vd, t);
|
||||
|
||||
return _mm_cvtss_f32(vd);
|
||||
#elif defined(B3_USE_NEON)
|
||||
float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
|
||||
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
|
||||
x = vpadd_f32(x, x);
|
||||
return vget_lane_f32(x, 0);
|
||||
#else
|
||||
return m_floats[0] * q.getX() +
|
||||
m_floats[1] * q.getY() +
|
||||
m_floats[2] * q.getZ() +
|
||||
m_floats[3] * q.m_floats[3];
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the length squared of the quaternion */
|
||||
b3Scalar length2() const
|
||||
{
|
||||
return dot(*this);
|
||||
}
|
||||
|
||||
/**@brief Return the length of the quaternion */
|
||||
b3Scalar length() const
|
||||
{
|
||||
return b3Sqrt(length2());
|
||||
}
|
||||
|
||||
/**@brief Normalize the quaternion
|
||||
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
|
||||
b3Quaternion& normalize()
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vd;
|
||||
|
||||
vd = _mm_mul_ps(mVec128, mVec128);
|
||||
|
||||
__m128 t = _mm_movehl_ps(vd, vd);
|
||||
vd = _mm_add_ps(vd, t);
|
||||
t = _mm_shuffle_ps(vd, vd, 0x55);
|
||||
vd = _mm_add_ss(vd, t);
|
||||
|
||||
vd = _mm_sqrt_ss(vd);
|
||||
vd = _mm_div_ss(b3vOnes, vd);
|
||||
vd = b3_pshufd_ps(vd, 0); // splat
|
||||
mVec128 = _mm_mul_ps(mVec128, vd);
|
||||
|
||||
return *this;
|
||||
#else
|
||||
return *this /= length();
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return a scaled version of this quaternion
|
||||
* @param s The scale factor */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Scalar& s) const
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
|
||||
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
|
||||
|
||||
return b3Quaternion(_mm_mul_ps(mVec128, vs));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion(vmulq_n_f32(mVec128, s));
|
||||
#else
|
||||
return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return an inversely scaled versionof this quaternion
|
||||
* @param s The inverse scale factor */
|
||||
b3Quaternion operator/(const b3Scalar& s) const
|
||||
{
|
||||
b3Assert(s != b3Scalar(0.0));
|
||||
return *this * (b3Scalar(1.0) / s);
|
||||
}
|
||||
|
||||
/**@brief Inversely scale this quaternion
|
||||
* @param s The scale factor */
|
||||
b3Quaternion& operator/=(const b3Scalar& s)
|
||||
{
|
||||
b3Assert(s != b3Scalar(0.0));
|
||||
return *this *= b3Scalar(1.0) / s;
|
||||
}
|
||||
|
||||
/**@brief Return a normalized version of this quaternion */
|
||||
b3Quaternion normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
/**@brief Return the angle between this quaternion and the other
|
||||
* @param q The other quaternion */
|
||||
b3Scalar angle(const b3Quaternion& q) const
|
||||
{
|
||||
b3Scalar s = b3Sqrt(length2() * q.length2());
|
||||
b3Assert(s != b3Scalar(0.0));
|
||||
return b3Acos(dot(q) / s);
|
||||
}
|
||||
/**@brief Return the angle of rotation represented by this quaternion */
|
||||
b3Scalar getAngle() const
|
||||
{
|
||||
b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]);
|
||||
return s;
|
||||
}
|
||||
|
||||
/**@brief Return the axis of the rotation represented by this quaternion */
|
||||
b3Vector3 getAxis() const
|
||||
{
|
||||
b3Scalar s_squared = 1.f - m_floats[3] * m_floats[3];
|
||||
|
||||
if (s_squared < b3Scalar(10.) * B3_EPSILON) //Check for divide by zero
|
||||
return b3MakeVector3(1.0, 0.0, 0.0); // Arbitrary
|
||||
b3Scalar s = 1.f / b3Sqrt(s_squared);
|
||||
return b3MakeVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
|
||||
}
|
||||
|
||||
/**@brief Return the inverse of this quaternion */
|
||||
b3Quaternion inverse() const
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
return b3Quaternion(_mm_xor_ps(mVec128, b3vQInv));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vQInv));
|
||||
#else
|
||||
return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the sum of this quaternion and the other
|
||||
* @param q2 The other quaternion */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator+(const b3Quaternion& q2) const
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
|
||||
#else
|
||||
const b3Quaternion& q1 = *this;
|
||||
return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the difference between this quaternion and the other
|
||||
* @param q2 The other quaternion */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator-(const b3Quaternion& q2) const
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
|
||||
#else
|
||||
const b3Quaternion& q1 = *this;
|
||||
return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Return the negative of this quaternion
|
||||
* This simply negates each element */
|
||||
B3_FORCE_INLINE b3Quaternion operator-() const
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask));
|
||||
#else
|
||||
const b3Quaternion& q2 = *this;
|
||||
return b3Quaternion(-q2.getX(), -q2.getY(), -q2.getZ(), -q2.m_floats[3]);
|
||||
#endif
|
||||
}
|
||||
/**@todo document this and it's use */
|
||||
B3_FORCE_INLINE b3Quaternion farthest(const b3Quaternion& qd) const
|
||||
{
|
||||
b3Quaternion diff, sum;
|
||||
diff = *this - qd;
|
||||
sum = *this + qd;
|
||||
if (diff.dot(diff) > sum.dot(sum))
|
||||
return qd;
|
||||
return (-qd);
|
||||
}
|
||||
|
||||
/**@todo document this and it's use */
|
||||
B3_FORCE_INLINE b3Quaternion nearest(const b3Quaternion& qd) const
|
||||
{
|
||||
b3Quaternion diff, sum;
|
||||
diff = *this - qd;
|
||||
sum = *this + qd;
|
||||
if (diff.dot(diff) < sum.dot(sum))
|
||||
return qd;
|
||||
return (-qd);
|
||||
}
|
||||
|
||||
/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
|
||||
* @param q The other quaternion to interpolate with
|
||||
* @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
|
||||
* Slerp interpolates assuming constant velocity. */
|
||||
b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
|
||||
{
|
||||
b3Scalar magnitude = b3Sqrt(length2() * q.length2());
|
||||
b3Assert(magnitude > b3Scalar(0));
|
||||
|
||||
b3Scalar product = dot(q) / magnitude;
|
||||
if (b3Fabs(product) < b3Scalar(1))
|
||||
{
|
||||
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
|
||||
const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
|
||||
|
||||
const b3Scalar theta = b3Acos(sign * product);
|
||||
const b3Scalar s1 = b3Sin(sign * t * theta);
|
||||
const b3Scalar d = b3Scalar(1.0) / b3Sin(theta);
|
||||
const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta);
|
||||
|
||||
return b3Quaternion(
|
||||
(m_floats[0] * s0 + q.getX() * s1) * d,
|
||||
(m_floats[1] * s0 + q.getY() * s1) * d,
|
||||
(m_floats[2] * s0 + q.getZ() * s1) * d,
|
||||
(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
|
||||
}
|
||||
else
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
}
|
||||
|
||||
static const b3Quaternion& getIdentity()
|
||||
{
|
||||
static const b3Quaternion identityQuat(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.), b3Scalar(1.));
|
||||
return identityQuat;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
|
||||
};
|
||||
|
||||
/**@brief Return the product of two quaternions */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Quaternion& q1, const b3Quaternion& q2)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vQ1 = q1.get128();
|
||||
__m128 vQ2 = q2.get128();
|
||||
__m128 A0, A1, B1, A2, B2;
|
||||
|
||||
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0)); // X Y z x // vtrn
|
||||
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0)); // W W W X // vdup vext
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1)); // Y Z X Y // vext
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1)); // z x Y Y // vtrn vdup
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2)); // z x Y Z // vtrn vext
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2)); // Y Z x z // vext vtrn
|
||||
|
||||
B1 = B1 * B2; // A3 *= B3
|
||||
|
||||
A0 = b3_splat_ps(vQ1, 3); // A0
|
||||
A0 = A0 * vQ2; // A0 * B0
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
A0 = A0 - B1; // AB03 = AB0 - AB3
|
||||
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
A0 = A0 + A1; // AB03 + AB12
|
||||
|
||||
return b3Quaternion(A0);
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = q1.get128();
|
||||
float32x4_t vQ2 = q2.get128();
|
||||
float32x4_t A0, A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
|
||||
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
}
|
||||
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
|
||||
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
A0 = vaddq_f32(A0, A1); // AB03 + AB12
|
||||
|
||||
return b3Quaternion(A0);
|
||||
|
||||
#else
|
||||
return b3Quaternion(
|
||||
q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
|
||||
q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
|
||||
q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
|
||||
q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Quaternion& q, const b3Vector3& w)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vQ1 = q.get128();
|
||||
__m128 vQ2 = w.get128();
|
||||
__m128 A1, B1, A2, B2, A3, B3;
|
||||
|
||||
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3, 3, 3, 0));
|
||||
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0, 1, 2, 0));
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
|
||||
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
|
||||
|
||||
A3 = A3 * B3; // A3 *= B3
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
A1 = A1 - A3; // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = q.get128();
|
||||
float32x4_t vQ2 = w.get128();
|
||||
float32x4_t A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
|
||||
|
||||
vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
|
||||
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
}
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X
|
||||
B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
|
||||
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#else
|
||||
return b3Quaternion(
|
||||
q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
|
||||
q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
|
||||
q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
|
||||
-q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
operator*(const b3Vector3& w, const b3Quaternion& q)
|
||||
{
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
__m128 vQ1 = w.get128();
|
||||
__m128 vQ2 = q.get128();
|
||||
__m128 A1, B1, A2, B2, A3, B3;
|
||||
|
||||
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0)); // X Y z x
|
||||
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0)); // W W W X
|
||||
|
||||
A1 = A1 * B1;
|
||||
|
||||
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
|
||||
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
|
||||
|
||||
A2 = A2 * B2;
|
||||
|
||||
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
|
||||
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
|
||||
|
||||
A3 = A3 * B3; // A3 *= B3
|
||||
|
||||
A1 = A1 + A2; // AB12
|
||||
A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element
|
||||
A1 = A1 - A3; // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#elif defined(B3_USE_NEON)
|
||||
|
||||
float32x4_t vQ1 = w.get128();
|
||||
float32x4_t vQ2 = q.get128();
|
||||
float32x4_t A1, B1, A2, B2, A3, B3;
|
||||
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
|
||||
|
||||
{
|
||||
float32x2x2_t tmp;
|
||||
|
||||
tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1)); // {z x}, {w y}
|
||||
vQ1zx = tmp.val[0];
|
||||
|
||||
tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2)); // {z x}, {w y}
|
||||
vQ2zx = tmp.val[0];
|
||||
}
|
||||
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
|
||||
|
||||
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
|
||||
|
||||
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
|
||||
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
|
||||
|
||||
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
|
||||
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
|
||||
|
||||
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
|
||||
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
|
||||
|
||||
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
|
||||
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
|
||||
|
||||
A1 = vmulq_f32(A1, B1);
|
||||
A2 = vmulq_f32(A2, B2);
|
||||
A3 = vmulq_f32(A3, B3); // A3 *= B3
|
||||
|
||||
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
|
||||
|
||||
// change the sign of the last element
|
||||
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
|
||||
|
||||
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
|
||||
|
||||
return b3Quaternion(A1);
|
||||
|
||||
#else
|
||||
return b3Quaternion(
|
||||
+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
|
||||
+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
|
||||
+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
|
||||
-w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
/**@brief Calculate the dot product between two quaternions */
|
||||
B3_FORCE_INLINE b3Scalar
|
||||
b3Dot(const b3Quaternion& q1, const b3Quaternion& q2)
|
||||
{
|
||||
return q1.dot(q2);
|
||||
}
|
||||
|
||||
/**@brief Return the length of a quaternion */
|
||||
B3_FORCE_INLINE b3Scalar
|
||||
b3Length(const b3Quaternion& q)
|
||||
{
|
||||
return q.length();
|
||||
}
|
||||
|
||||
/**@brief Return the angle between two quaternions*/
|
||||
B3_FORCE_INLINE b3Scalar
|
||||
b3Angle(const b3Quaternion& q1, const b3Quaternion& q2)
|
||||
{
|
||||
return q1.angle(q2);
|
||||
}
|
||||
|
||||
/**@brief Return the inverse of a quaternion*/
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3Inverse(const b3Quaternion& q)
|
||||
{
|
||||
return q.inverse();
|
||||
}
|
||||
|
||||
/**@brief Return the result of spherical linear interpolation betwen two quaternions
|
||||
* @param q1 The first quaternion
|
||||
* @param q2 The second quaternion
|
||||
* @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
|
||||
* Slerp assumes constant velocity between positions. */
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3Slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t)
|
||||
{
|
||||
return q1.slerp(q2, t);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1)
|
||||
{
|
||||
return rot0 * rot1;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3QuatNormalized(const b3Quaternion& orn)
|
||||
{
|
||||
return orn.normalized();
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Vector3
|
||||
b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
|
||||
{
|
||||
b3Quaternion q = rotation * v;
|
||||
q *= rotation.inverse();
|
||||
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
|
||||
return b3MakeVector3(_mm_and_ps(q.get128(), b3vFFF0fMask));
|
||||
#elif defined(B3_USE_NEON)
|
||||
return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask));
|
||||
#else
|
||||
return b3MakeVector3(q.getX(), q.getY(), q.getZ());
|
||||
#endif
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
|
||||
{
|
||||
b3Vector3 c = v0.cross(v1);
|
||||
b3Scalar d = v0.dot(v1);
|
||||
|
||||
if (d < -1.0 + B3_EPSILON)
|
||||
{
|
||||
b3Vector3 n, unused;
|
||||
b3PlaneSpace1(v0, n, unused);
|
||||
return b3Quaternion(n.getX(), n.getY(), n.getZ(), 0.0f); // just pick any vector that is orthogonal to v0
|
||||
}
|
||||
|
||||
b3Scalar s = b3Sqrt((1.0f + d) * 2.0f);
|
||||
b3Scalar rs = 1.0f / s;
|
||||
|
||||
return b3Quaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Quaternion
|
||||
b3ShortestArcQuatNormalize2(b3Vector3& v0, b3Vector3& v1)
|
||||
{
|
||||
v0.normalize();
|
||||
v1.normalize();
|
||||
return b3ShortestArcQuat(v0, v1);
|
||||
}
|
||||
|
||||
#endif //B3_SIMD__QUATERNION_H_
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_GEN_RANDOM_H
|
||||
#define B3_GEN_RANDOM_H
|
||||
|
||||
#include "b3Scalar.h"
|
||||
|
||||
#ifdef MT19937
|
||||
|
||||
#include <limits.h>
|
||||
#include <mt19937.h>
|
||||
|
||||
#define B3_RAND_MAX UINT_MAX
|
||||
|
||||
B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
|
||||
B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
|
||||
|
||||
#else
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#define B3_RAND_MAX RAND_MAX
|
||||
|
||||
B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
|
||||
B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
|
||||
|
||||
#endif
|
||||
|
||||
inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
|
||||
{
|
||||
return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
|
||||
}
|
||||
|
||||
#endif //B3_GEN_RANDOM_H
|
||||
|
|
@ -1,171 +0,0 @@
|
|||
|
||||
#ifndef B3_RESIZABLE_POOL_H
|
||||
#define B3_RESIZABLE_POOL_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
enum
|
||||
{
|
||||
B3_POOL_HANDLE_TERMINAL_FREE = -1,
|
||||
B3_POOL_HANDLE_TERMINAL_USED = -2
|
||||
};
|
||||
|
||||
template <typename U>
|
||||
struct b3PoolBodyHandle : public U
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
int m_nextFreeHandle;
|
||||
void setNextFree(int next)
|
||||
{
|
||||
m_nextFreeHandle = next;
|
||||
}
|
||||
int getNextFree() const
|
||||
{
|
||||
return m_nextFreeHandle;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class b3ResizablePool
|
||||
{
|
||||
protected:
|
||||
b3AlignedObjectArray<T> m_bodyHandles;
|
||||
int m_numUsedHandles; // number of active handles
|
||||
int m_firstFreeHandle; // free handles list
|
||||
|
||||
T* getHandleInternal(int handle)
|
||||
{
|
||||
return &m_bodyHandles[handle];
|
||||
}
|
||||
const T* getHandleInternal(int handle) const
|
||||
{
|
||||
return &m_bodyHandles[handle];
|
||||
}
|
||||
|
||||
public:
|
||||
b3ResizablePool()
|
||||
{
|
||||
initHandles();
|
||||
}
|
||||
|
||||
virtual ~b3ResizablePool()
|
||||
{
|
||||
exitHandles();
|
||||
}
|
||||
///handle management
|
||||
|
||||
int getNumHandles() const
|
||||
{
|
||||
return m_bodyHandles.size();
|
||||
}
|
||||
|
||||
void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
|
||||
{
|
||||
for (int i = 0; i < m_bodyHandles.size(); i++)
|
||||
{
|
||||
if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
|
||||
{
|
||||
usedHandles.push_back(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
T* getHandle(int handle)
|
||||
{
|
||||
b3Assert(handle >= 0);
|
||||
b3Assert(handle < m_bodyHandles.size());
|
||||
if ((handle < 0) || (handle >= m_bodyHandles.size()))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
|
||||
{
|
||||
return &m_bodyHandles[handle];
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
const T* getHandle(int handle) const
|
||||
{
|
||||
b3Assert(handle >= 0);
|
||||
b3Assert(handle < m_bodyHandles.size());
|
||||
if ((handle < 0) || (handle >= m_bodyHandles.size()))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
|
||||
{
|
||||
return &m_bodyHandles[handle];
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void increaseHandleCapacity(int extraCapacity)
|
||||
{
|
||||
int curCapacity = m_bodyHandles.size();
|
||||
//b3Assert(curCapacity == m_numUsedHandles);
|
||||
int newCapacity = curCapacity + extraCapacity;
|
||||
m_bodyHandles.resize(newCapacity);
|
||||
|
||||
{
|
||||
for (int i = curCapacity; i < newCapacity; i++)
|
||||
m_bodyHandles[i].setNextFree(i + 1);
|
||||
|
||||
m_bodyHandles[newCapacity - 1].setNextFree(-1);
|
||||
}
|
||||
m_firstFreeHandle = curCapacity;
|
||||
}
|
||||
void initHandles()
|
||||
{
|
||||
m_numUsedHandles = 0;
|
||||
m_firstFreeHandle = -1;
|
||||
|
||||
increaseHandleCapacity(1);
|
||||
}
|
||||
|
||||
void exitHandles()
|
||||
{
|
||||
m_bodyHandles.resize(0);
|
||||
m_firstFreeHandle = -1;
|
||||
m_numUsedHandles = 0;
|
||||
}
|
||||
|
||||
int allocHandle()
|
||||
{
|
||||
b3Assert(m_firstFreeHandle >= 0);
|
||||
|
||||
int handle = m_firstFreeHandle;
|
||||
m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
|
||||
m_numUsedHandles++;
|
||||
|
||||
if (m_firstFreeHandle < 0)
|
||||
{
|
||||
//int curCapacity = m_bodyHandles.size();
|
||||
int additionalCapacity = m_bodyHandles.size();
|
||||
increaseHandleCapacity(additionalCapacity);
|
||||
|
||||
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
|
||||
}
|
||||
getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
|
||||
getHandleInternal(handle)->clear();
|
||||
return handle;
|
||||
}
|
||||
|
||||
void freeHandle(int handle)
|
||||
{
|
||||
b3Assert(handle >= 0);
|
||||
|
||||
if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
|
||||
{
|
||||
getHandleInternal(handle)->clear();
|
||||
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
|
||||
m_firstFreeHandle = handle;
|
||||
m_numUsedHandles--;
|
||||
}
|
||||
}
|
||||
};
|
||||
///end handle management
|
||||
|
||||
#endif //B3_RESIZABLE_POOL_H
|
||||
|
|
@ -1,685 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_SCALAR_H
|
||||
#define B3_SCALAR_H
|
||||
|
||||
#ifdef B3_MANAGED_CODE
|
||||
//Aligned data types not supported in managed code
|
||||
#pragma unmanaged
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
#include <stdlib.h> //size_t for MSVC 6.0
|
||||
#include <float.h>
|
||||
|
||||
//Original repository is at http://github.com/erwincoumans/bullet3
|
||||
#define B3_BULLET_VERSION 300
|
||||
|
||||
inline int b3GetVersion()
|
||||
{
|
||||
return B3_BULLET_VERSION;
|
||||
}
|
||||
|
||||
#if defined(DEBUG) || defined(_DEBUG)
|
||||
#define B3_DEBUG
|
||||
#endif
|
||||
|
||||
#include "b3Logging.h" //for b3Error
|
||||
|
||||
#ifdef _WIN32
|
||||
|
||||
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
|
||||
|
||||
#define B3_FORCE_INLINE inline
|
||||
#define B3_ATTRIBUTE_ALIGNED16(a) a
|
||||
#define B3_ATTRIBUTE_ALIGNED64(a) a
|
||||
#define B3_ATTRIBUTE_ALIGNED128(a) a
|
||||
#else
|
||||
//#define B3_HAS_ALIGNED_ALLOCATOR
|
||||
#pragma warning(disable : 4324) // disable padding warning
|
||||
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
|
||||
#pragma warning(disable : 4996) //Turn off warnings about deprecated C routines
|
||||
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
|
||||
|
||||
#define B3_FORCE_INLINE __forceinline
|
||||
#define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
|
||||
#define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
|
||||
#define B3_ATTRIBUTE_ALIGNED128(a) __declspec(align(128)) a
|
||||
#ifdef _XBOX
|
||||
#define B3_USE_VMX128
|
||||
|
||||
#include <ppcintrinsics.h>
|
||||
#define B3_HAVE_NATIVE_FSEL
|
||||
#define b3Fsel(a, b, c) __fsel((a), (b), (c))
|
||||
#else
|
||||
|
||||
#if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(B3_USE_DOUBLE_PRECISION))
|
||||
#if (defined(_M_IX86) || defined(_M_X64))
|
||||
#define B3_USE_SSE
|
||||
#ifdef B3_USE_SSE
|
||||
//B3_USE_SSE_IN_API is disabled under Windows by default, because
|
||||
//it makes it harder to integrate Bullet into your application under Windows
|
||||
//(structured embedding Bullet structs/classes need to be 16-byte aligned)
|
||||
//with relatively little performance gain
|
||||
//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
|
||||
//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
|
||||
//#define B3_USE_SSE_IN_API
|
||||
#endif //B3_USE_SSE
|
||||
#include <emmintrin.h>
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif //_XBOX
|
||||
|
||||
#endif //__MINGW32__
|
||||
|
||||
#ifdef B3_DEBUG
|
||||
#ifdef _MSC_VER
|
||||
#include <stdio.h>
|
||||
#define b3Assert(x) \
|
||||
{ \
|
||||
if (!(x)) \
|
||||
{ \
|
||||
b3Error( \
|
||||
"Assert "__FILE__ \
|
||||
":%u (" #x ")\n", \
|
||||
__LINE__); \
|
||||
__debugbreak(); \
|
||||
} \
|
||||
}
|
||||
#else //_MSC_VER
|
||||
#include <assert.h>
|
||||
#define b3Assert assert
|
||||
#endif //_MSC_VER
|
||||
#else
|
||||
#define b3Assert(x)
|
||||
#endif
|
||||
//b3FullAssert is optional, slows down a lot
|
||||
#define b3FullAssert(x)
|
||||
|
||||
#define b3Likely(_c) _c
|
||||
#define b3Unlikely(_c) _c
|
||||
|
||||
#else
|
||||
|
||||
#if defined(__CELLOS_LV2__)
|
||||
#define B3_FORCE_INLINE inline __attribute__((always_inline))
|
||||
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
|
||||
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
|
||||
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
|
||||
#ifndef assert
|
||||
#include <assert.h>
|
||||
#endif
|
||||
#ifdef B3_DEBUG
|
||||
#ifdef __SPU__
|
||||
#include <spu_printf.h>
|
||||
#define printf spu_printf
|
||||
#define b3Assert(x) \
|
||||
{ \
|
||||
if (!(x)) \
|
||||
{ \
|
||||
b3Error( \
|
||||
"Assert "__FILE__ \
|
||||
":%u (" #x ")\n", \
|
||||
__LINE__); \
|
||||
spu_hcmpeq(0, 0); \
|
||||
} \
|
||||
}
|
||||
#else
|
||||
#define b3Assert assert
|
||||
#endif
|
||||
|
||||
#else
|
||||
#define b3Assert(x)
|
||||
#endif
|
||||
//b3FullAssert is optional, slows down a lot
|
||||
#define b3FullAssert(x)
|
||||
|
||||
#define b3Likely(_c) _c
|
||||
#define b3Unlikely(_c) _c
|
||||
|
||||
#else
|
||||
|
||||
#ifdef USE_LIBSPE2
|
||||
|
||||
#define B3_FORCE_INLINE __inline
|
||||
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
|
||||
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
|
||||
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
|
||||
#ifndef assert
|
||||
#include <assert.h>
|
||||
#endif
|
||||
#ifdef B3_DEBUG
|
||||
#define b3Assert assert
|
||||
#else
|
||||
#define b3Assert(x)
|
||||
#endif
|
||||
//b3FullAssert is optional, slows down a lot
|
||||
#define b3FullAssert(x)
|
||||
|
||||
#define b3Likely(_c) __builtin_expect((_c), 1)
|
||||
#define b3Unlikely(_c) __builtin_expect((_c), 0)
|
||||
|
||||
#else
|
||||
//non-windows systems
|
||||
|
||||
#if (defined(__APPLE__) && (!defined(B3_USE_DOUBLE_PRECISION)))
|
||||
#if defined(__i386__) || defined(__x86_64__)
|
||||
#define B3_USE_SSE
|
||||
//B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
|
||||
//if apps run into issues, we will disable the next line
|
||||
#define B3_USE_SSE_IN_API
|
||||
#ifdef B3_USE_SSE
|
||||
// include appropriate SSE level
|
||||
#if defined(__SSE4_1__)
|
||||
#include <smmintrin.h>
|
||||
#elif defined(__SSSE3__)
|
||||
#include <tmmintrin.h>
|
||||
#elif defined(__SSE3__)
|
||||
#include <pmmintrin.h>
|
||||
#else
|
||||
#include <emmintrin.h>
|
||||
#endif
|
||||
#endif //B3_USE_SSE
|
||||
#elif defined(__armv7__)
|
||||
#ifdef __clang__
|
||||
#define B3_USE_NEON 1
|
||||
|
||||
#if defined B3_USE_NEON && defined(__clang__)
|
||||
#include <arm_neon.h>
|
||||
#endif //B3_USE_NEON
|
||||
#endif //__clang__
|
||||
#endif //__arm__
|
||||
|
||||
#define B3_FORCE_INLINE inline __attribute__((always_inline))
|
||||
///@todo: check out alignment methods for other platforms/compilers
|
||||
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
|
||||
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
|
||||
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
|
||||
#ifndef assert
|
||||
#include <assert.h>
|
||||
#endif
|
||||
|
||||
#if defined(DEBUG) || defined(_DEBUG)
|
||||
#if defined(__i386__) || defined(__x86_64__)
|
||||
#include <stdio.h>
|
||||
#define b3Assert(x) \
|
||||
{ \
|
||||
if (!(x)) \
|
||||
{ \
|
||||
b3Error("Assert %s in line %d, file %s\n", #x, __LINE__, __FILE__); \
|
||||
asm volatile("int3"); \
|
||||
} \
|
||||
}
|
||||
#else //defined (__i386__) || defined (__x86_64__)
|
||||
#define b3Assert assert
|
||||
#endif //defined (__i386__) || defined (__x86_64__)
|
||||
#else //defined(DEBUG) || defined (_DEBUG)
|
||||
#define b3Assert(x)
|
||||
#endif //defined(DEBUG) || defined (_DEBUG)
|
||||
|
||||
//b3FullAssert is optional, slows down a lot
|
||||
#define b3FullAssert(x)
|
||||
#define b3Likely(_c) _c
|
||||
#define b3Unlikely(_c) _c
|
||||
|
||||
#else
|
||||
|
||||
#define B3_FORCE_INLINE inline
|
||||
///@todo: check out alignment methods for other platforms/compilers
|
||||
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
|
||||
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
|
||||
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
|
||||
///#define B3_ATTRIBUTE_ALIGNED16(a) a
|
||||
///#define B3_ATTRIBUTE_ALIGNED64(a) a
|
||||
///#define B3_ATTRIBUTE_ALIGNED128(a) a
|
||||
#ifndef assert
|
||||
#include <assert.h>
|
||||
#endif
|
||||
|
||||
#if defined(DEBUG) || defined(_DEBUG)
|
||||
#define b3Assert assert
|
||||
#else
|
||||
#define b3Assert(x)
|
||||
#endif
|
||||
|
||||
//b3FullAssert is optional, slows down a lot
|
||||
#define b3FullAssert(x)
|
||||
#define b3Likely(_c) _c
|
||||
#define b3Unlikely(_c) _c
|
||||
#endif //__APPLE__
|
||||
|
||||
#endif // LIBSPE2
|
||||
|
||||
#endif //__CELLOS_LV2__
|
||||
#endif
|
||||
|
||||
///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
|
||||
#if defined(B3_USE_DOUBLE_PRECISION)
|
||||
typedef double b3Scalar;
|
||||
//this number could be bigger in double precision
|
||||
#define B3_LARGE_FLOAT 1e30
|
||||
#else
|
||||
typedef float b3Scalar;
|
||||
//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
|
||||
#define B3_LARGE_FLOAT 1e18f
|
||||
#endif
|
||||
|
||||
#ifdef B3_USE_SSE
|
||||
typedef __m128 b3SimdFloat4;
|
||||
#endif //B3_USE_SSE
|
||||
|
||||
#if defined B3_USE_SSE_IN_API && defined(B3_USE_SSE)
|
||||
#ifdef _WIN32
|
||||
|
||||
#ifndef B3_NAN
|
||||
static int b3NanMask = 0x7F800001;
|
||||
#define B3_NAN (*(float *)&b3NanMask)
|
||||
#endif
|
||||
|
||||
#ifndef B3_INFINITY_MASK
|
||||
static int b3InfinityMask = 0x7F800000;
|
||||
#define B3_INFINITY_MASK (*(float *)&b3InfinityMask)
|
||||
#endif
|
||||
|
||||
inline __m128 operator+(const __m128 A, const __m128 B)
|
||||
{
|
||||
return _mm_add_ps(A, B);
|
||||
}
|
||||
|
||||
inline __m128 operator-(const __m128 A, const __m128 B)
|
||||
{
|
||||
return _mm_sub_ps(A, B);
|
||||
}
|
||||
|
||||
inline __m128 operator*(const __m128 A, const __m128 B)
|
||||
{
|
||||
return _mm_mul_ps(A, B);
|
||||
}
|
||||
|
||||
#define b3CastfTo128i(a) (_mm_castps_si128(a))
|
||||
#define b3CastfTo128d(a) (_mm_castps_pd(a))
|
||||
#define b3CastiTo128f(a) (_mm_castsi128_ps(a))
|
||||
#define b3CastdTo128f(a) (_mm_castpd_ps(a))
|
||||
#define b3CastdTo128i(a) (_mm_castpd_si128(a))
|
||||
#define b3Assign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)
|
||||
|
||||
#else //_WIN32
|
||||
|
||||
#define b3CastfTo128i(a) ((__m128i)(a))
|
||||
#define b3CastfTo128d(a) ((__m128d)(a))
|
||||
#define b3CastiTo128f(a) ((__m128)(a))
|
||||
#define b3CastdTo128f(a) ((__m128)(a))
|
||||
#define b3CastdTo128i(a) ((__m128i)(a))
|
||||
#define b3Assign128(r0, r1, r2, r3) \
|
||||
(__m128) { r0, r1, r2, r3 }
|
||||
#endif //_WIN32
|
||||
#endif //B3_USE_SSE_IN_API
|
||||
|
||||
#ifdef B3_USE_NEON
|
||||
#include <arm_neon.h>
|
||||
|
||||
typedef float32x4_t b3SimdFloat4;
|
||||
#define B3_INFINITY INFINITY
|
||||
#define B3_NAN NAN
|
||||
#define b3Assign128(r0, r1, r2, r3) \
|
||||
(float32x4_t) { r0, r1, r2, r3 }
|
||||
#endif
|
||||
|
||||
#define B3_DECLARE_ALIGNED_ALLOCATOR() \
|
||||
B3_FORCE_INLINE void *operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
|
||||
B3_FORCE_INLINE void operator delete(void *ptr) { b3AlignedFree(ptr); } \
|
||||
B3_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; } \
|
||||
B3_FORCE_INLINE void operator delete(void *, void *) {} \
|
||||
B3_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
|
||||
B3_FORCE_INLINE void operator delete[](void *ptr) { b3AlignedFree(ptr); } \
|
||||
B3_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; } \
|
||||
B3_FORCE_INLINE void operator delete[](void *, void *) {}
|
||||
|
||||
#if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
|
||||
|
||||
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
|
||||
{
|
||||
return sqrt(x);
|
||||
}
|
||||
B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
|
||||
{
|
||||
if (x < b3Scalar(-1)) x = b3Scalar(-1);
|
||||
if (x > b3Scalar(1)) x = b3Scalar(1);
|
||||
return acos(x);
|
||||
}
|
||||
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
|
||||
{
|
||||
if (x < b3Scalar(-1)) x = b3Scalar(-1);
|
||||
if (x > b3Scalar(1)) x = b3Scalar(1);
|
||||
return asin(x);
|
||||
}
|
||||
B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
|
||||
B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return pow(x, y); }
|
||||
B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmod(x, y); }
|
||||
|
||||
#else
|
||||
|
||||
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
|
||||
{
|
||||
#ifdef USE_APPROXIMATION
|
||||
double x, z, tempf;
|
||||
unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
|
||||
|
||||
tempf = y;
|
||||
*tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
|
||||
x = tempf;
|
||||
z = y * b3Scalar(0.5);
|
||||
x = (b3Scalar(1.5) * x) - (x * x) * (x * z); /* iteration formula */
|
||||
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
|
||||
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
|
||||
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
|
||||
x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
|
||||
return x * y;
|
||||
#else
|
||||
return sqrtf(y);
|
||||
#endif
|
||||
}
|
||||
B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
|
||||
{
|
||||
if (x < b3Scalar(-1))
|
||||
x = b3Scalar(-1);
|
||||
if (x > b3Scalar(1))
|
||||
x = b3Scalar(1);
|
||||
return acosf(x);
|
||||
}
|
||||
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
|
||||
{
|
||||
if (x < b3Scalar(-1))
|
||||
x = b3Scalar(-1);
|
||||
if (x > b3Scalar(1))
|
||||
x = b3Scalar(1);
|
||||
return asinf(x);
|
||||
}
|
||||
B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
|
||||
B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
|
||||
B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return powf(x, y); }
|
||||
B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmodf(x, y); }
|
||||
|
||||
#endif
|
||||
|
||||
#define B3_2_PI b3Scalar(6.283185307179586232)
|
||||
#define B3_PI (B3_2_PI * b3Scalar(0.5))
|
||||
#define B3_HALF_PI (B3_2_PI * b3Scalar(0.25))
|
||||
#define B3_RADS_PER_DEG (B3_2_PI / b3Scalar(360.0))
|
||||
#define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI)
|
||||
#define B3_SQRT12 b3Scalar(0.7071067811865475244008443621048490)
|
||||
|
||||
#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0) / b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
|
||||
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
#define B3_EPSILON DBL_EPSILON
|
||||
#define B3_INFINITY DBL_MAX
|
||||
#else
|
||||
#define B3_EPSILON FLT_EPSILON
|
||||
#define B3_INFINITY FLT_MAX
|
||||
#endif
|
||||
|
||||
B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
|
||||
{
|
||||
b3Scalar coeff_1 = B3_PI / 4.0f;
|
||||
b3Scalar coeff_2 = 3.0f * coeff_1;
|
||||
b3Scalar abs_y = b3Fabs(y);
|
||||
b3Scalar angle;
|
||||
if (x >= 0.0f)
|
||||
{
|
||||
b3Scalar r = (x - abs_y) / (x + abs_y);
|
||||
angle = coeff_1 - coeff_1 * r;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Scalar r = (x + abs_y) / (abs_y - x);
|
||||
angle = coeff_2 - coeff_1 * r;
|
||||
}
|
||||
return (y < 0.0f) ? -angle : angle;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
|
||||
|
||||
B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
|
||||
{
|
||||
return (((a) <= eps) && !((a) < -eps));
|
||||
}
|
||||
B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
|
||||
{
|
||||
return (!((a) <= eps));
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
|
||||
{
|
||||
return x < b3Scalar(0.0) ? 1 : 0;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
|
||||
B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; }
|
||||
|
||||
#define B3_DECLARE_HANDLE(name) \
|
||||
typedef struct name##__ \
|
||||
{ \
|
||||
int unused; \
|
||||
} * name
|
||||
|
||||
#ifndef b3Fsel
|
||||
B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
|
||||
{
|
||||
return a >= 0 ? b : c;
|
||||
}
|
||||
#endif
|
||||
#define b3Fsels(a, b, c) (b3Scalar) b3Fsel(a, b, c)
|
||||
|
||||
B3_FORCE_INLINE bool b3MachineIsLittleEndian()
|
||||
{
|
||||
long int i = 1;
|
||||
const char *p = (const char *)&i;
|
||||
if (p[0] == 1) // Lowest address contains the least significant byte
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
///b3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
|
||||
///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
|
||||
B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
|
||||
{
|
||||
// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
|
||||
// Rely on positive value or'ed with its negative having sign bit on
|
||||
// and zero value or'ed with its negative (which is still zero) having sign bit off
|
||||
// Use arithmetic shift right, shifting the sign bit through all 32 bits
|
||||
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
|
||||
unsigned testEqz = ~testNz;
|
||||
return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
|
||||
}
|
||||
B3_FORCE_INLINE int b3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
|
||||
{
|
||||
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
|
||||
unsigned testEqz = ~testNz;
|
||||
return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
|
||||
}
|
||||
B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
|
||||
{
|
||||
#ifdef B3_HAVE_NATIVE_FSEL
|
||||
return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
|
||||
#else
|
||||
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
|
||||
#endif
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
B3_FORCE_INLINE void b3Swap(T &a, T &b)
|
||||
{
|
||||
T tmp = a;
|
||||
a = b;
|
||||
b = tmp;
|
||||
}
|
||||
|
||||
//PCK: endian swapping functions
|
||||
B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
|
||||
{
|
||||
return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val)
|
||||
{
|
||||
return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE unsigned b3SwapEndian(int val)
|
||||
{
|
||||
return b3SwapEndian((unsigned)val);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE unsigned short b3SwapEndian(short val)
|
||||
{
|
||||
return b3SwapEndian((unsigned short)val);
|
||||
}
|
||||
|
||||
///b3SwapFloat uses using char pointers to swap the endianness
|
||||
////b3SwapFloat/b3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
|
||||
///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
|
||||
///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
|
||||
///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
|
||||
///so instead of returning a float/double, we return integer/long long integer
|
||||
B3_FORCE_INLINE unsigned int b3SwapEndianFloat(float d)
|
||||
{
|
||||
unsigned int a = 0;
|
||||
unsigned char *dst = (unsigned char *)&a;
|
||||
unsigned char *src = (unsigned char *)&d;
|
||||
|
||||
dst[0] = src[3];
|
||||
dst[1] = src[2];
|
||||
dst[2] = src[1];
|
||||
dst[3] = src[0];
|
||||
return a;
|
||||
}
|
||||
|
||||
// unswap using char pointers
|
||||
B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
|
||||
{
|
||||
float d = 0.0f;
|
||||
unsigned char *src = (unsigned char *)&a;
|
||||
unsigned char *dst = (unsigned char *)&d;
|
||||
|
||||
dst[0] = src[3];
|
||||
dst[1] = src[2];
|
||||
dst[2] = src[1];
|
||||
dst[3] = src[0];
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
// swap using char pointers
|
||||
B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char *dst)
|
||||
{
|
||||
unsigned char *src = (unsigned char *)&d;
|
||||
|
||||
dst[0] = src[7];
|
||||
dst[1] = src[6];
|
||||
dst[2] = src[5];
|
||||
dst[3] = src[4];
|
||||
dst[4] = src[3];
|
||||
dst[5] = src[2];
|
||||
dst[6] = src[1];
|
||||
dst[7] = src[0];
|
||||
}
|
||||
|
||||
// unswap using char pointers
|
||||
B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src)
|
||||
{
|
||||
double d = 0.0;
|
||||
unsigned char *dst = (unsigned char *)&d;
|
||||
|
||||
dst[0] = src[7];
|
||||
dst[1] = src[6];
|
||||
dst[2] = src[5];
|
||||
dst[3] = src[4];
|
||||
dst[4] = src[3];
|
||||
dst[5] = src[2];
|
||||
dst[6] = src[1];
|
||||
dst[7] = src[0];
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
// returns normalized value in range [-B3_PI, B3_PI]
|
||||
B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
|
||||
{
|
||||
angleInRadians = b3Fmod(angleInRadians, B3_2_PI);
|
||||
if (angleInRadians < -B3_PI)
|
||||
{
|
||||
return angleInRadians + B3_2_PI;
|
||||
}
|
||||
else if (angleInRadians > B3_PI)
|
||||
{
|
||||
return angleInRadians - B3_2_PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
}
|
||||
|
||||
///rudimentary class to provide type info
|
||||
struct b3TypedObject
|
||||
{
|
||||
b3TypedObject(int objectType)
|
||||
: m_objectType(objectType)
|
||||
{
|
||||
}
|
||||
int m_objectType;
|
||||
inline int getObjectType() const
|
||||
{
|
||||
return m_objectType;
|
||||
}
|
||||
};
|
||||
|
||||
///align a pointer to the provided alignment, upwards
|
||||
template <typename T>
|
||||
T *b3AlignPointer(T *unalignedPtr, size_t alignment)
|
||||
{
|
||||
struct b3ConvertPointerSizeT
|
||||
{
|
||||
union {
|
||||
T *ptr;
|
||||
size_t integer;
|
||||
};
|
||||
};
|
||||
b3ConvertPointerSizeT converter;
|
||||
|
||||
const size_t bit_mask = ~(alignment - 1);
|
||||
converter.ptr = unalignedPtr;
|
||||
converter.integer += alignment - 1;
|
||||
converter.integer &= bit_mask;
|
||||
return converter.ptr;
|
||||
}
|
||||
|
||||
#endif //B3_SCALAR_H
|
||||
|
|
@ -1,118 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
/*
|
||||
StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
|
||||
Nov.2006
|
||||
*/
|
||||
|
||||
#ifndef B3_STACK_ALLOC
|
||||
#define B3_STACK_ALLOC
|
||||
|
||||
#include "b3Scalar.h" //for b3Assert
|
||||
#include "b3AlignedAllocator.h"
|
||||
|
||||
///The b3Block class is an internal structure for the b3StackAlloc memory allocator.
|
||||
struct b3Block
|
||||
{
|
||||
b3Block* previous;
|
||||
unsigned char* address;
|
||||
};
|
||||
|
||||
///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
|
||||
class b3StackAlloc
|
||||
{
|
||||
public:
|
||||
b3StackAlloc(unsigned int size)
|
||||
{
|
||||
ctor();
|
||||
create(size);
|
||||
}
|
||||
~b3StackAlloc() { destroy(); }
|
||||
|
||||
inline void create(unsigned int size)
|
||||
{
|
||||
destroy();
|
||||
data = (unsigned char*)b3AlignedAlloc(size, 16);
|
||||
totalsize = size;
|
||||
}
|
||||
inline void destroy()
|
||||
{
|
||||
b3Assert(usedsize == 0);
|
||||
//Raise(L"StackAlloc is still in use");
|
||||
|
||||
if (usedsize == 0)
|
||||
{
|
||||
if (!ischild && data)
|
||||
b3AlignedFree(data);
|
||||
|
||||
data = 0;
|
||||
usedsize = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int getAvailableMemory() const
|
||||
{
|
||||
return static_cast<int>(totalsize - usedsize);
|
||||
}
|
||||
|
||||
unsigned char* allocate(unsigned int size)
|
||||
{
|
||||
const unsigned int nus(usedsize + size);
|
||||
if (nus < totalsize)
|
||||
{
|
||||
usedsize = nus;
|
||||
return (data + (usedsize - size));
|
||||
}
|
||||
b3Assert(0);
|
||||
//&& (L"Not enough memory"));
|
||||
|
||||
return (0);
|
||||
}
|
||||
B3_FORCE_INLINE b3Block* beginBlock()
|
||||
{
|
||||
b3Block* pb = (b3Block*)allocate(sizeof(b3Block));
|
||||
pb->previous = current;
|
||||
pb->address = data + usedsize;
|
||||
current = pb;
|
||||
return (pb);
|
||||
}
|
||||
B3_FORCE_INLINE void endBlock(b3Block* block)
|
||||
{
|
||||
b3Assert(block == current);
|
||||
//Raise(L"Unmatched blocks");
|
||||
if (block == current)
|
||||
{
|
||||
current = block->previous;
|
||||
usedsize = (unsigned int)((block->address - data) - sizeof(b3Block));
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void ctor()
|
||||
{
|
||||
data = 0;
|
||||
totalsize = 0;
|
||||
usedsize = 0;
|
||||
current = 0;
|
||||
ischild = false;
|
||||
}
|
||||
unsigned char* data;
|
||||
unsigned int totalsize;
|
||||
unsigned int usedsize;
|
||||
b3Block* current;
|
||||
bool ischild;
|
||||
};
|
||||
|
||||
#endif //B3_STACK_ALLOC
|
||||
|
|
@ -1,286 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_TRANSFORM_H
|
||||
#define B3_TRANSFORM_H
|
||||
|
||||
#include "b3Matrix3x3.h"
|
||||
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
#define b3TransformData b3TransformDoubleData
|
||||
#else
|
||||
#define b3TransformData b3TransformFloatData
|
||||
#endif
|
||||
|
||||
/**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
|
||||
*It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3Transform
|
||||
{
|
||||
///Storage for the rotation
|
||||
b3Matrix3x3 m_basis;
|
||||
///Storage for the translation
|
||||
b3Vector3 m_origin;
|
||||
|
||||
public:
|
||||
/**@brief No initialization constructor */
|
||||
b3Transform() {}
|
||||
/**@brief Constructor from b3Quaternion (optional b3Vector3 )
|
||||
* @param q Rotation from quaternion
|
||||
* @param c Translation from Vector (default 0,0,0) */
|
||||
explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q,
|
||||
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
|
||||
: m_basis(q),
|
||||
m_origin(c)
|
||||
{
|
||||
}
|
||||
|
||||
/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
|
||||
* @param b Rotation from Matrix
|
||||
* @param c Translation from Vector default (0,0,0)*/
|
||||
explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b,
|
||||
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
|
||||
: m_basis(b),
|
||||
m_origin(c)
|
||||
{
|
||||
}
|
||||
/**@brief Copy constructor */
|
||||
B3_FORCE_INLINE b3Transform(const b3Transform& other)
|
||||
: m_basis(other.m_basis),
|
||||
m_origin(other.m_origin)
|
||||
{
|
||||
}
|
||||
/**@brief Assignment Operator */
|
||||
B3_FORCE_INLINE b3Transform& operator=(const b3Transform& other)
|
||||
{
|
||||
m_basis = other.m_basis;
|
||||
m_origin = other.m_origin;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Set the current transform as the value of the product of two transforms
|
||||
* @param t1 Transform 1
|
||||
* @param t2 Transform 2
|
||||
* This = Transform1 * Transform2 */
|
||||
B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2)
|
||||
{
|
||||
m_basis = t1.m_basis * t2.m_basis;
|
||||
m_origin = t1(t2.m_origin);
|
||||
}
|
||||
|
||||
/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
|
||||
b3Vector3 v = t2.m_origin - t1.m_origin;
|
||||
m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis);
|
||||
m_origin = v * t1.m_basis;
|
||||
}
|
||||
*/
|
||||
|
||||
/**@brief Return the transform of the vector */
|
||||
B3_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const
|
||||
{
|
||||
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
|
||||
}
|
||||
|
||||
/**@brief Return the transform of the vector */
|
||||
B3_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const
|
||||
{
|
||||
return (*this)(x);
|
||||
}
|
||||
|
||||
/**@brief Return the transform of the b3Quaternion */
|
||||
B3_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const
|
||||
{
|
||||
return getRotation() * q;
|
||||
}
|
||||
|
||||
/**@brief Return the basis matrix for the rotation */
|
||||
B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; }
|
||||
/**@brief Return the basis matrix for the rotation */
|
||||
B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; }
|
||||
|
||||
/**@brief Return the origin vector translation */
|
||||
B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; }
|
||||
/**@brief Return the origin vector translation */
|
||||
B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
|
||||
|
||||
/**@brief Return a quaternion representing the rotation */
|
||||
b3Quaternion getRotation() const
|
||||
{
|
||||
b3Quaternion q;
|
||||
m_basis.getRotation(q);
|
||||
return q;
|
||||
}
|
||||
|
||||
/**@brief Set from an array
|
||||
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
|
||||
void setFromOpenGLMatrix(const b3Scalar* m)
|
||||
{
|
||||
m_basis.setFromOpenGLSubMatrix(m);
|
||||
m_origin.setValue(m[12], m[13], m[14]);
|
||||
}
|
||||
|
||||
/**@brief Fill an array representation
|
||||
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
|
||||
void getOpenGLMatrix(b3Scalar * m) const
|
||||
{
|
||||
m_basis.getOpenGLSubMatrix(m);
|
||||
m[12] = m_origin.getX();
|
||||
m[13] = m_origin.getY();
|
||||
m[14] = m_origin.getZ();
|
||||
m[15] = b3Scalar(1.0);
|
||||
}
|
||||
|
||||
/**@brief Set the translational element
|
||||
* @param origin The vector to set the translation to */
|
||||
B3_FORCE_INLINE void setOrigin(const b3Vector3& origin)
|
||||
{
|
||||
m_origin = origin;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const;
|
||||
|
||||
/**@brief Set the rotational element by b3Matrix3x3 */
|
||||
B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis)
|
||||
{
|
||||
m_basis = basis;
|
||||
}
|
||||
|
||||
/**@brief Set the rotational element by b3Quaternion */
|
||||
B3_FORCE_INLINE void setRotation(const b3Quaternion& q)
|
||||
{
|
||||
m_basis.setRotation(q);
|
||||
}
|
||||
|
||||
/**@brief Set this transformation to the identity */
|
||||
void setIdentity()
|
||||
{
|
||||
m_basis.setIdentity();
|
||||
m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0));
|
||||
}
|
||||
|
||||
/**@brief Multiply this Transform by another(this = this * another)
|
||||
* @param t The other transform */
|
||||
b3Transform& operator*=(const b3Transform& t)
|
||||
{
|
||||
m_origin += m_basis * t.m_origin;
|
||||
m_basis *= t.m_basis;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**@brief Return the inverse of this transform */
|
||||
b3Transform inverse() const
|
||||
{
|
||||
b3Matrix3x3 inv = m_basis.transpose();
|
||||
return b3Transform(inv, inv * -m_origin);
|
||||
}
|
||||
|
||||
/**@brief Return the inverse of this transform times the other transform
|
||||
* @param t The other transform
|
||||
* return this.inverse() * the other */
|
||||
b3Transform inverseTimes(const b3Transform& t) const;
|
||||
|
||||
/**@brief Return the product of this transform and the other */
|
||||
b3Transform operator*(const b3Transform& t) const;
|
||||
|
||||
/**@brief Return an identity transform */
|
||||
static const b3Transform& getIdentity()
|
||||
{
|
||||
static const b3Transform identityTransform(b3Matrix3x3::getIdentity());
|
||||
return identityTransform;
|
||||
}
|
||||
|
||||
void serialize(struct b3TransformData & dataOut) const;
|
||||
|
||||
void serializeFloat(struct b3TransformFloatData & dataOut) const;
|
||||
|
||||
void deSerialize(const struct b3TransformData& dataIn);
|
||||
|
||||
void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
|
||||
|
||||
void deSerializeFloat(const struct b3TransformFloatData& dataIn);
|
||||
};
|
||||
|
||||
B3_FORCE_INLINE b3Vector3
|
||||
b3Transform::invXform(const b3Vector3& inVec) const
|
||||
{
|
||||
b3Vector3 v = inVec - m_origin;
|
||||
return (m_basis.transpose() * v);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Transform
|
||||
b3Transform::inverseTimes(const b3Transform& t) const
|
||||
{
|
||||
b3Vector3 v = t.getOrigin() - m_origin;
|
||||
return b3Transform(m_basis.transposeTimes(t.m_basis),
|
||||
v * m_basis);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3Transform
|
||||
b3Transform::operator*(const b3Transform& t) const
|
||||
{
|
||||
return b3Transform(m_basis * t.m_basis,
|
||||
(*this)(t.m_origin));
|
||||
}
|
||||
|
||||
/**@brief Test if two transforms have all elements equal */
|
||||
B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
|
||||
{
|
||||
return (t1.getBasis() == t2.getBasis() &&
|
||||
t1.getOrigin() == t2.getOrigin());
|
||||
}
|
||||
|
||||
///for serialization
|
||||
struct b3TransformFloatData
|
||||
{
|
||||
b3Matrix3x3FloatData m_basis;
|
||||
b3Vector3FloatData m_origin;
|
||||
};
|
||||
|
||||
struct b3TransformDoubleData
|
||||
{
|
||||
b3Matrix3x3DoubleData m_basis;
|
||||
b3Vector3DoubleData m_origin;
|
||||
};
|
||||
|
||||
B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
|
||||
{
|
||||
m_basis.serialize(dataOut.m_basis);
|
||||
m_origin.serialize(dataOut.m_origin);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
|
||||
{
|
||||
m_basis.serializeFloat(dataOut.m_basis);
|
||||
m_origin.serializeFloat(dataOut.m_origin);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
|
||||
{
|
||||
m_basis.deSerialize(dataIn.m_basis);
|
||||
m_origin.deSerialize(dataIn.m_origin);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
|
||||
{
|
||||
m_basis.deSerializeFloat(dataIn.m_basis);
|
||||
m_origin.deSerializeFloat(dataIn.m_origin);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
|
||||
{
|
||||
m_basis.deSerializeDouble(dataIn.m_basis);
|
||||
m_origin.deSerializeDouble(dataIn.m_origin);
|
||||
}
|
||||
|
||||
#endif //B3_TRANSFORM_H
|
||||
|
|
@ -1,210 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_TRANSFORM_UTIL_H
|
||||
#define B3_TRANSFORM_UTIL_H
|
||||
|
||||
#include "b3Transform.h"
|
||||
#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
|
||||
|
||||
B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
|
||||
{
|
||||
return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
|
||||
supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
|
||||
supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
|
||||
}
|
||||
|
||||
/// Utils related to temporal transforms
|
||||
class b3TransformUtil
|
||||
{
|
||||
public:
|
||||
static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
|
||||
{
|
||||
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
|
||||
// #define QUATERNION_DERIVATIVE
|
||||
#ifdef QUATERNION_DERIVATIVE
|
||||
b3Quaternion predictedOrn = curTrans.getRotation();
|
||||
predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
|
||||
predictedOrn.normalize();
|
||||
#else
|
||||
//Exponential map
|
||||
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
|
||||
|
||||
b3Vector3 axis;
|
||||
b3Scalar fAngle = angvel.length();
|
||||
//limit the angular motion
|
||||
if (fAngle * timeStep > B3_ANGULAR_MOTION_THRESHOLD)
|
||||
{
|
||||
fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep;
|
||||
}
|
||||
|
||||
if (fAngle < b3Scalar(0.001))
|
||||
{
|
||||
// use Taylor's expansions of sync function
|
||||
axis = angvel * (b3Scalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (b3Scalar(0.020833333333)) * fAngle * fAngle);
|
||||
}
|
||||
else
|
||||
{
|
||||
// sync(fAngle) = sin(c*fAngle)/t
|
||||
axis = angvel * (b3Sin(b3Scalar(0.5) * fAngle * timeStep) / fAngle);
|
||||
}
|
||||
b3Quaternion dorn(axis.getX(), axis.getY(), axis.getZ(), b3Cos(fAngle * timeStep * b3Scalar(0.5)));
|
||||
b3Quaternion orn0 = curTrans.getRotation();
|
||||
|
||||
b3Quaternion predictedOrn = dorn * orn0;
|
||||
predictedOrn.normalize();
|
||||
#endif
|
||||
predictedTransform.setRotation(predictedOrn);
|
||||
}
|
||||
|
||||
static void calculateVelocityQuaternion(const b3Vector3& pos0, const b3Vector3& pos1, const b3Quaternion& orn0, const b3Quaternion& orn1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
|
||||
{
|
||||
linVel = (pos1 - pos0) / timeStep;
|
||||
b3Vector3 axis;
|
||||
b3Scalar angle;
|
||||
if (orn0 != orn1)
|
||||
{
|
||||
calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
|
||||
angVel = axis * angle / timeStep;
|
||||
}
|
||||
else
|
||||
{
|
||||
angVel.setValue(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0, const b3Quaternion& orn1a, b3Vector3& axis, b3Scalar& angle)
|
||||
{
|
||||
b3Quaternion orn1 = orn0.nearest(orn1a);
|
||||
b3Quaternion dorn = orn1 * orn0.inverse();
|
||||
angle = dorn.getAngle();
|
||||
axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
|
||||
axis[3] = b3Scalar(0.);
|
||||
//check for axis length
|
||||
b3Scalar len = axis.length2();
|
||||
if (len < B3_EPSILON * B3_EPSILON)
|
||||
axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
|
||||
else
|
||||
axis /= b3Sqrt(len);
|
||||
}
|
||||
|
||||
static void calculateVelocity(const b3Transform& transform0, const b3Transform& transform1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
|
||||
{
|
||||
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
|
||||
b3Vector3 axis;
|
||||
b3Scalar angle;
|
||||
calculateDiffAxisAngle(transform0, transform1, axis, angle);
|
||||
angVel = axis * angle / timeStep;
|
||||
}
|
||||
|
||||
static void calculateDiffAxisAngle(const b3Transform& transform0, const b3Transform& transform1, b3Vector3& axis, b3Scalar& angle)
|
||||
{
|
||||
b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
|
||||
b3Quaternion dorn;
|
||||
dmat.getRotation(dorn);
|
||||
|
||||
///floating point inaccuracy can lead to w component > 1..., which breaks
|
||||
dorn.normalize();
|
||||
|
||||
angle = dorn.getAngle();
|
||||
axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
|
||||
axis[3] = b3Scalar(0.);
|
||||
//check for axis length
|
||||
b3Scalar len = axis.length2();
|
||||
if (len < B3_EPSILON * B3_EPSILON)
|
||||
axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
|
||||
else
|
||||
axis /= b3Sqrt(len);
|
||||
}
|
||||
};
|
||||
|
||||
///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
|
||||
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
|
||||
class b3ConvexSeparatingDistanceUtil
|
||||
{
|
||||
b3Quaternion m_ornA;
|
||||
b3Quaternion m_ornB;
|
||||
b3Vector3 m_posA;
|
||||
b3Vector3 m_posB;
|
||||
|
||||
b3Vector3 m_separatingNormal;
|
||||
|
||||
b3Scalar m_boundingRadiusA;
|
||||
b3Scalar m_boundingRadiusB;
|
||||
b3Scalar m_separatingDistance;
|
||||
|
||||
public:
|
||||
b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
|
||||
: m_boundingRadiusA(boundingRadiusA),
|
||||
m_boundingRadiusB(boundingRadiusB),
|
||||
m_separatingDistance(0.f)
|
||||
{
|
||||
}
|
||||
|
||||
b3Scalar getConservativeSeparatingDistance()
|
||||
{
|
||||
return m_separatingDistance;
|
||||
}
|
||||
|
||||
void updateSeparatingDistance(const b3Transform& transA, const b3Transform& transB)
|
||||
{
|
||||
const b3Vector3& toPosA = transA.getOrigin();
|
||||
const b3Vector3& toPosB = transB.getOrigin();
|
||||
b3Quaternion toOrnA = transA.getRotation();
|
||||
b3Quaternion toOrnB = transB.getRotation();
|
||||
|
||||
if (m_separatingDistance > 0.f)
|
||||
{
|
||||
b3Vector3 linVelA, angVelA, linVelB, angVelB;
|
||||
b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
|
||||
b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
|
||||
b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
|
||||
b3Vector3 relLinVel = (linVelB - linVelA);
|
||||
b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
|
||||
if (relLinVelocLength < 0.f)
|
||||
{
|
||||
relLinVelocLength = 0.f;
|
||||
}
|
||||
|
||||
b3Scalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
|
||||
m_separatingDistance -= projectedMotion;
|
||||
}
|
||||
|
||||
m_posA = toPosA;
|
||||
m_posB = toPosB;
|
||||
m_ornA = toOrnA;
|
||||
m_ornB = toOrnB;
|
||||
}
|
||||
|
||||
void initSeparatingDistance(const b3Vector3& separatingVector, b3Scalar separatingDistance, const b3Transform& transA, const b3Transform& transB)
|
||||
{
|
||||
m_separatingDistance = separatingDistance;
|
||||
|
||||
if (m_separatingDistance > 0.f)
|
||||
{
|
||||
m_separatingNormal = separatingVector;
|
||||
|
||||
const b3Vector3& toPosA = transA.getOrigin();
|
||||
const b3Vector3& toPosB = transB.getOrigin();
|
||||
b3Quaternion toOrnA = transA.getRotation();
|
||||
b3Quaternion toOrnB = transB.getRotation();
|
||||
m_posA = toPosA;
|
||||
m_posB = toPosB;
|
||||
m_ornA = toOrnA;
|
||||
m_ornB = toOrnB;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_TRANSFORM_UTIL_H
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -1,16 +0,0 @@
|
|||
project "Bullet3Common"
|
||||
|
||||
language "C++"
|
||||
|
||||
kind "StaticLib"
|
||||
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
|
||||
includedirs {".."}
|
||||
|
||||
files {
|
||||
"*.cpp",
|
||||
"*.h"
|
||||
}
|
||||
|
|
@ -1,90 +0,0 @@
|
|||
#ifndef B3_FLOAT4_H
|
||||
#define B3_FLOAT4_H
|
||||
|
||||
#include "Bullet3Common/shared/b3PlatformDefinitions.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#define b3Float4 b3Vector3
|
||||
#define b3Float4ConstArg const b3Vector3&
|
||||
#define b3Dot3F4 b3Dot
|
||||
#define b3Cross3 b3Cross
|
||||
#define b3MakeFloat4 b3MakeVector3
|
||||
inline b3Vector3 b3Normalized(const b3Vector3& vec)
|
||||
{
|
||||
return vec.normalized();
|
||||
}
|
||||
|
||||
inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
|
||||
{
|
||||
return v.normalized();
|
||||
}
|
||||
|
||||
inline b3Float4 b3MaxFloat4(const b3Float4& a, const b3Float4& b)
|
||||
{
|
||||
b3Float4 tmp = a;
|
||||
tmp.setMax(b);
|
||||
return tmp;
|
||||
}
|
||||
inline b3Float4 b3MinFloat4(const b3Float4& a, const b3Float4& b)
|
||||
{
|
||||
b3Float4 tmp = a;
|
||||
tmp.setMin(b);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
#else
|
||||
typedef float4 b3Float4;
|
||||
#define b3Float4ConstArg const b3Float4
|
||||
#define b3MakeFloat4 (float4)
|
||||
float b3Dot3F4(b3Float4ConstArg v0, b3Float4ConstArg v1)
|
||||
{
|
||||
float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
|
||||
float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
|
||||
return dot(a1, b1);
|
||||
}
|
||||
b3Float4 b3Cross3(b3Float4ConstArg v0, b3Float4ConstArg v1)
|
||||
{
|
||||
float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
|
||||
float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
|
||||
return cross(a1, b1);
|
||||
}
|
||||
#define b3MinFloat4 min
|
||||
#define b3MaxFloat4 max
|
||||
|
||||
#define b3Normalized(a) normalize(a)
|
||||
|
||||
#endif
|
||||
|
||||
inline bool b3IsAlmostZero(b3Float4ConstArg v)
|
||||
{
|
||||
if (b3Fabs(v.x) > 1e-6 || b3Fabs(v.y) > 1e-6 || b3Fabs(v.z) > 1e-6)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline int b3MaxDot(b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut)
|
||||
{
|
||||
float maxDot = -B3_INFINITY;
|
||||
int i = 0;
|
||||
int ptIndex = -1;
|
||||
for (i = 0; i < vecLen; i++)
|
||||
{
|
||||
float dot = b3Dot3F4(vecArray[i], vec);
|
||||
|
||||
if (dot > maxDot)
|
||||
{
|
||||
maxDot = dot;
|
||||
ptIndex = i;
|
||||
}
|
||||
}
|
||||
b3Assert(ptIndex >= 0);
|
||||
if (ptIndex < 0)
|
||||
{
|
||||
ptIndex = 0;
|
||||
}
|
||||
*dotOut = maxDot;
|
||||
return ptIndex;
|
||||
}
|
||||
|
||||
#endif //B3_FLOAT4_H
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_INT2_H
|
||||
#define B3_INT2_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
struct b3UnsignedInt2
|
||||
{
|
||||
union {
|
||||
struct
|
||||
{
|
||||
unsigned int x, y;
|
||||
};
|
||||
struct
|
||||
{
|
||||
unsigned int s[2];
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
struct b3Int2
|
||||
{
|
||||
union {
|
||||
struct
|
||||
{
|
||||
int x, y;
|
||||
};
|
||||
struct
|
||||
{
|
||||
int s[2];
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
inline b3Int2 b3MakeInt2(int x, int y)
|
||||
{
|
||||
b3Int2 v;
|
||||
v.s[0] = x;
|
||||
v.s[1] = y;
|
||||
return v;
|
||||
}
|
||||
#else
|
||||
|
||||
#define b3UnsignedInt2 uint2
|
||||
#define b3Int2 int2
|
||||
#define b3MakeInt2 (int2)
|
||||
|
||||
#endif //__cplusplus
|
||||
#endif
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
#ifndef B3_INT4_H
|
||||
#define B3_INT4_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3UnsignedInt4
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
union {
|
||||
struct
|
||||
{
|
||||
unsigned int x, y, z, w;
|
||||
};
|
||||
struct
|
||||
{
|
||||
unsigned int s[4];
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3Int4
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
union {
|
||||
struct
|
||||
{
|
||||
int x, y, z, w;
|
||||
};
|
||||
struct
|
||||
{
|
||||
int s[4];
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
B3_FORCE_INLINE b3Int4 b3MakeInt4(int x, int y, int z, int w = 0)
|
||||
{
|
||||
b3Int4 v;
|
||||
v.s[0] = x;
|
||||
v.s[1] = y;
|
||||
v.s[2] = z;
|
||||
v.s[3] = w;
|
||||
return v;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y, unsigned int z, unsigned int w = 0)
|
||||
{
|
||||
b3UnsignedInt4 v;
|
||||
v.s[0] = x;
|
||||
v.s[1] = y;
|
||||
v.s[2] = z;
|
||||
v.s[3] = w;
|
||||
return v;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
#define b3UnsignedInt4 uint4
|
||||
#define b3Int4 int4
|
||||
#define b3MakeInt4 (int4)
|
||||
#define b3MakeUnsignedInt4 (uint4)
|
||||
|
||||
#endif //__cplusplus
|
||||
|
||||
#endif //B3_INT4_H
|
||||
|
|
@ -1,157 +0,0 @@
|
|||
|
||||
#ifndef B3_MAT3x3_H
|
||||
#define B3_MAT3x3_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Quat.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
|
||||
#define b3Mat3x3 b3Matrix3x3
|
||||
#define b3Mat3x3ConstArg const b3Matrix3x3&
|
||||
|
||||
inline b3Mat3x3 b3QuatGetRotationMatrix(b3QuatConstArg quat)
|
||||
{
|
||||
return b3Mat3x3(quat);
|
||||
}
|
||||
|
||||
inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat)
|
||||
{
|
||||
return mat.absolute();
|
||||
}
|
||||
|
||||
#define b3GetRow(m, row) m.getRow(row)
|
||||
|
||||
__inline b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
|
||||
{
|
||||
return b * a;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
typedef struct
|
||||
{
|
||||
b3Float4 m_row[3];
|
||||
} b3Mat3x3;
|
||||
|
||||
#define b3Mat3x3ConstArg const b3Mat3x3
|
||||
#define b3GetRow(m, row) (m.m_row[row])
|
||||
|
||||
inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)
|
||||
{
|
||||
b3Float4 quat2 = (b3Float4)(quat.x * quat.x, quat.y * quat.y, quat.z * quat.z, 0.f);
|
||||
b3Mat3x3 out;
|
||||
|
||||
out.m_row[0].x = 1 - 2 * quat2.y - 2 * quat2.z;
|
||||
out.m_row[0].y = 2 * quat.x * quat.y - 2 * quat.w * quat.z;
|
||||
out.m_row[0].z = 2 * quat.x * quat.z + 2 * quat.w * quat.y;
|
||||
out.m_row[0].w = 0.f;
|
||||
|
||||
out.m_row[1].x = 2 * quat.x * quat.y + 2 * quat.w * quat.z;
|
||||
out.m_row[1].y = 1 - 2 * quat2.x - 2 * quat2.z;
|
||||
out.m_row[1].z = 2 * quat.y * quat.z - 2 * quat.w * quat.x;
|
||||
out.m_row[1].w = 0.f;
|
||||
|
||||
out.m_row[2].x = 2 * quat.x * quat.z - 2 * quat.w * quat.y;
|
||||
out.m_row[2].y = 2 * quat.y * quat.z + 2 * quat.w * quat.x;
|
||||
out.m_row[2].z = 1 - 2 * quat2.x - 2 * quat2.y;
|
||||
out.m_row[2].w = 0.f;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)
|
||||
{
|
||||
b3Mat3x3 out;
|
||||
out.m_row[0] = fabs(matIn.m_row[0]);
|
||||
out.m_row[1] = fabs(matIn.m_row[1]);
|
||||
out.m_row[2] = fabs(matIn.m_row[2]);
|
||||
return out;
|
||||
}
|
||||
|
||||
__inline b3Mat3x3 mtZero();
|
||||
|
||||
__inline b3Mat3x3 mtIdentity();
|
||||
|
||||
__inline b3Mat3x3 mtTranspose(b3Mat3x3 m);
|
||||
|
||||
__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
|
||||
|
||||
__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
|
||||
|
||||
__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
|
||||
|
||||
__inline b3Mat3x3 mtZero()
|
||||
{
|
||||
b3Mat3x3 m;
|
||||
m.m_row[0] = (b3Float4)(0.f);
|
||||
m.m_row[1] = (b3Float4)(0.f);
|
||||
m.m_row[2] = (b3Float4)(0.f);
|
||||
return m;
|
||||
}
|
||||
|
||||
__inline b3Mat3x3 mtIdentity()
|
||||
{
|
||||
b3Mat3x3 m;
|
||||
m.m_row[0] = (b3Float4)(1, 0, 0, 0);
|
||||
m.m_row[1] = (b3Float4)(0, 1, 0, 0);
|
||||
m.m_row[2] = (b3Float4)(0, 0, 1, 0);
|
||||
return m;
|
||||
}
|
||||
|
||||
__inline b3Mat3x3 mtTranspose(b3Mat3x3 m)
|
||||
{
|
||||
b3Mat3x3 out;
|
||||
out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
|
||||
out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
|
||||
out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
|
||||
return out;
|
||||
}
|
||||
|
||||
__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
|
||||
{
|
||||
b3Mat3x3 transB;
|
||||
transB = mtTranspose(b);
|
||||
b3Mat3x3 ans;
|
||||
// why this doesn't run when 0ing in the for{}
|
||||
a.m_row[0].w = 0.f;
|
||||
a.m_row[1].w = 0.f;
|
||||
a.m_row[2].w = 0.f;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
// a.m_row[i].w = 0.f;
|
||||
ans.m_row[i].x = b3Dot3F4(a.m_row[i], transB.m_row[0]);
|
||||
ans.m_row[i].y = b3Dot3F4(a.m_row[i], transB.m_row[1]);
|
||||
ans.m_row[i].z = b3Dot3F4(a.m_row[i], transB.m_row[2]);
|
||||
ans.m_row[i].w = 0.f;
|
||||
}
|
||||
return ans;
|
||||
}
|
||||
|
||||
__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
|
||||
{
|
||||
b3Float4 ans;
|
||||
ans.x = b3Dot3F4(a.m_row[0], b);
|
||||
ans.y = b3Dot3F4(a.m_row[1], b);
|
||||
ans.z = b3Dot3F4(a.m_row[2], b);
|
||||
ans.w = 0.f;
|
||||
return ans;
|
||||
}
|
||||
|
||||
__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
|
||||
{
|
||||
b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
|
||||
b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
|
||||
b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
|
||||
|
||||
b3Float4 ans;
|
||||
ans.x = b3Dot3F4(a, colx);
|
||||
ans.y = b3Dot3F4(a, coly);
|
||||
ans.z = b3Dot3F4(a, colz);
|
||||
return ans;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#endif //B3_MAT3x3_H
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
#ifndef B3_PLATFORM_DEFINITIONS_H
|
||||
#define B3_PLATFORM_DEFINITIONS_H
|
||||
|
||||
struct MyTest
|
||||
{
|
||||
int bla;
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
//#define b3ConstArray(a) const b3AlignedObjectArray<a>&
|
||||
#define b3ConstArray(a) const a *
|
||||
#define b3AtomicInc(a) ((*a)++)
|
||||
|
||||
inline int b3AtomicAdd(volatile int *p, int val)
|
||||
{
|
||||
int oldValue = *p;
|
||||
int newValue = oldValue + val;
|
||||
*p = newValue;
|
||||
return oldValue;
|
||||
}
|
||||
|
||||
#define __global
|
||||
|
||||
#define B3_STATIC static
|
||||
#else
|
||||
//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
|
||||
#define B3_LARGE_FLOAT 1e18f
|
||||
#define B3_INFINITY 1e18f
|
||||
#define b3Assert(a)
|
||||
#define b3ConstArray(a) __global const a *
|
||||
#define b3AtomicInc atomic_inc
|
||||
#define b3AtomicAdd atomic_add
|
||||
#define b3Fabs fabs
|
||||
#define b3Sqrt native_sqrt
|
||||
#define b3Sin native_sin
|
||||
#define b3Cos native_cos
|
||||
|
||||
#define B3_STATIC
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -1,100 +0,0 @@
|
|||
#ifndef B3_QUAT_H
|
||||
#define B3_QUAT_H
|
||||
|
||||
#include "Bullet3Common/shared/b3PlatformDefinitions.h"
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
#include "Bullet3Common/b3Quaternion.h"
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
|
||||
#define b3Quat b3Quaternion
|
||||
#define b3QuatConstArg const b3Quaternion&
|
||||
inline b3Quat b3QuatInverse(b3QuatConstArg orn)
|
||||
{
|
||||
return orn.inverse();
|
||||
}
|
||||
|
||||
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
|
||||
{
|
||||
b3Transform tr;
|
||||
tr.setOrigin(translation);
|
||||
tr.setRotation(orientation);
|
||||
return tr(point);
|
||||
}
|
||||
|
||||
#else
|
||||
typedef float4 b3Quat;
|
||||
#define b3QuatConstArg const b3Quat
|
||||
|
||||
inline float4 b3FastNormalize4(float4 v)
|
||||
{
|
||||
v = (float4)(v.xyz, 0.f);
|
||||
return fast_normalize(v);
|
||||
}
|
||||
|
||||
inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
|
||||
inline b3Quat b3QuatNormalized(b3QuatConstArg in);
|
||||
inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
|
||||
inline b3Quat b3QuatInvert(b3QuatConstArg q);
|
||||
inline b3Quat b3QuatInverse(b3QuatConstArg q);
|
||||
|
||||
inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
|
||||
{
|
||||
b3Quat ans;
|
||||
ans = b3Cross3(a, b);
|
||||
ans += a.w * b + b.w * a;
|
||||
// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
|
||||
ans.w = a.w * b.w - b3Dot3F4(a, b);
|
||||
return ans;
|
||||
}
|
||||
|
||||
inline b3Quat b3QuatNormalized(b3QuatConstArg in)
|
||||
{
|
||||
b3Quat q;
|
||||
q = in;
|
||||
//return b3FastNormalize4(in);
|
||||
float len = native_sqrt(dot(q, q));
|
||||
if (len > 0.f)
|
||||
{
|
||||
q *= 1.f / len;
|
||||
}
|
||||
else
|
||||
{
|
||||
q.x = q.y = q.z = 0.f;
|
||||
q.w = 1.f;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
|
||||
{
|
||||
b3Quat qInv = b3QuatInvert(q);
|
||||
float4 vcpy = vec;
|
||||
vcpy.w = 0.f;
|
||||
float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv);
|
||||
return out;
|
||||
}
|
||||
|
||||
inline b3Quat b3QuatInverse(b3QuatConstArg q)
|
||||
{
|
||||
return (b3Quat)(-q.xyz, q.w);
|
||||
}
|
||||
|
||||
inline b3Quat b3QuatInvert(b3QuatConstArg q)
|
||||
{
|
||||
return (b3Quat)(-q.xyz, q.w);
|
||||
}
|
||||
|
||||
inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
|
||||
{
|
||||
return b3QuatRotate(b3QuatInvert(q), vec);
|
||||
}
|
||||
|
||||
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
|
||||
{
|
||||
return b3QuatRotate(orientation, point) + (translation);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#endif //B3_QUAT_H
|
||||
|
|
@ -1,61 +0,0 @@
|
|||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
)
|
||||
|
||||
SET(Bullet3Dynamics_SRCS
|
||||
b3CpuRigidBodyPipeline.cpp
|
||||
ConstraintSolver/b3FixedConstraint.cpp
|
||||
ConstraintSolver/b3Generic6DofConstraint.cpp
|
||||
ConstraintSolver/b3PgsJacobiSolver.cpp
|
||||
ConstraintSolver/b3Point2PointConstraint.cpp
|
||||
ConstraintSolver/b3TypedConstraint.cpp
|
||||
)
|
||||
|
||||
SET(Bullet3Dynamics_HDRS
|
||||
b3CpuRigidBodyPipeline.h
|
||||
ConstraintSolver/b3ContactSolverInfo.h
|
||||
ConstraintSolver/b3FixedConstraint.h
|
||||
ConstraintSolver/b3Generic6DofConstraint.h
|
||||
ConstraintSolver/b3JacobianEntry.h
|
||||
ConstraintSolver/b3PgsJacobiSolver.h
|
||||
ConstraintSolver/b3Point2PointConstraint.h
|
||||
ConstraintSolver/b3SolverBody.h
|
||||
ConstraintSolver/b3SolverConstraint.h
|
||||
ConstraintSolver/b3TypedConstraint.h
|
||||
shared/b3ContactConstraint4.h
|
||||
shared/b3ConvertConstraint4.h
|
||||
shared/b3Inertia.h
|
||||
shared/b3IntegrateTransforms.h
|
||||
)
|
||||
|
||||
ADD_LIBRARY(Bullet3Dynamics ${Bullet3Dynamics_SRCS} ${Bullet3Dynamics_HDRS})
|
||||
if (BUILD_SHARED_LIBS)
|
||||
target_link_libraries(Bullet3Dynamics Bullet3Collision)
|
||||
endif ()
|
||||
SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#FILES_MATCHING requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Dynamics DESTINATION .)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Dynamics
|
||||
RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
|
||||
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES FRAMEWORK true)
|
||||
SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES PUBLIC_HEADER "${Bullet3Dynamics_HDRS}")
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
||||
|
|
@ -1,149 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_CONTACT_SOLVER_INFO
|
||||
#define B3_CONTACT_SOLVER_INFO
|
||||
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
|
||||
enum b3SolverMode
|
||||
{
|
||||
B3_SOLVER_RANDMIZE_ORDER = 1,
|
||||
B3_SOLVER_FRICTION_SEPARATE = 2,
|
||||
B3_SOLVER_USE_WARMSTARTING = 4,
|
||||
B3_SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
|
||||
B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
|
||||
B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
|
||||
B3_SOLVER_CACHE_FRIENDLY = 128,
|
||||
B3_SOLVER_SIMD = 256,
|
||||
B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
|
||||
B3_SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
|
||||
};
|
||||
|
||||
struct b3ContactSolverInfoData
|
||||
{
|
||||
b3Scalar m_tau;
|
||||
b3Scalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
b3Scalar m_friction;
|
||||
b3Scalar m_timeStep;
|
||||
b3Scalar m_restitution;
|
||||
int m_numIterations;
|
||||
b3Scalar m_maxErrorReduction;
|
||||
b3Scalar m_sor;
|
||||
b3Scalar m_erp; //used as Baumgarte factor
|
||||
b3Scalar m_erp2; //used in Split Impulse
|
||||
b3Scalar m_globalCfm; //constraint force mixing
|
||||
int m_splitImpulse;
|
||||
b3Scalar m_splitImpulsePenetrationThreshold;
|
||||
b3Scalar m_splitImpulseTurnErp;
|
||||
b3Scalar m_linearSlop;
|
||||
b3Scalar m_warmstartingFactor;
|
||||
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
b3Scalar m_maxGyroscopicForce;
|
||||
b3Scalar m_singleAxisRollingFrictionThreshold;
|
||||
};
|
||||
|
||||
struct b3ContactSolverInfo : public b3ContactSolverInfoData
|
||||
{
|
||||
inline b3ContactSolverInfo()
|
||||
{
|
||||
m_tau = b3Scalar(0.6);
|
||||
m_damping = b3Scalar(1.0);
|
||||
m_friction = b3Scalar(0.3);
|
||||
m_timeStep = b3Scalar(1.f / 60.f);
|
||||
m_restitution = b3Scalar(0.);
|
||||
m_maxErrorReduction = b3Scalar(20.);
|
||||
m_numIterations = 10;
|
||||
m_erp = b3Scalar(0.2);
|
||||
m_erp2 = b3Scalar(0.8);
|
||||
m_globalCfm = b3Scalar(0.);
|
||||
m_sor = b3Scalar(1.);
|
||||
m_splitImpulse = true;
|
||||
m_splitImpulsePenetrationThreshold = -.04f;
|
||||
m_splitImpulseTurnErp = 0.1f;
|
||||
m_linearSlop = b3Scalar(0.0);
|
||||
m_warmstartingFactor = b3Scalar(0.85);
|
||||
//m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
|
||||
m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD; // | B3_SOLVER_RANDMIZE_ORDER;
|
||||
m_restingContactRestitutionThreshold = 2; //unused as of 2.81
|
||||
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
|
||||
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
|
||||
m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
|
||||
}
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct b3ContactSolverInfoDoubleData
|
||||
{
|
||||
double m_tau;
|
||||
double m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
double m_friction;
|
||||
double m_timeStep;
|
||||
double m_restitution;
|
||||
double m_maxErrorReduction;
|
||||
double m_sor;
|
||||
double m_erp; //used as Baumgarte factor
|
||||
double m_erp2; //used in Split Impulse
|
||||
double m_globalCfm; //constraint force mixing
|
||||
double m_splitImpulsePenetrationThreshold;
|
||||
double m_splitImpulseTurnErp;
|
||||
double m_linearSlop;
|
||||
double m_warmstartingFactor;
|
||||
double m_maxGyroscopicForce;
|
||||
double m_singleAxisRollingFrictionThreshold;
|
||||
|
||||
int m_numIterations;
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
int m_splitImpulse;
|
||||
char m_padding[4];
|
||||
};
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct b3ContactSolverInfoFloatData
|
||||
{
|
||||
float m_tau;
|
||||
float m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
float m_friction;
|
||||
float m_timeStep;
|
||||
|
||||
float m_restitution;
|
||||
float m_maxErrorReduction;
|
||||
float m_sor;
|
||||
float m_erp; //used as Baumgarte factor
|
||||
|
||||
float m_erp2; //used in Split Impulse
|
||||
float m_globalCfm; //constraint force mixing
|
||||
float m_splitImpulsePenetrationThreshold;
|
||||
float m_splitImpulseTurnErp;
|
||||
|
||||
float m_linearSlop;
|
||||
float m_warmstartingFactor;
|
||||
float m_maxGyroscopicForce;
|
||||
float m_singleAxisRollingFrictionThreshold;
|
||||
|
||||
int m_numIterations;
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
|
||||
int m_splitImpulse;
|
||||
char m_padding[4];
|
||||
};
|
||||
|
||||
#endif //B3_CONTACT_SOLVER_INFO
|
||||
|
|
@ -1,103 +0,0 @@
|
|||
|
||||
#include "b3FixedConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
|
||||
: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
|
||||
{
|
||||
m_pivotInA = frameInA.getOrigin();
|
||||
m_pivotInB = frameInB.getOrigin();
|
||||
m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
|
||||
}
|
||||
|
||||
b3FixedConstraint::~b3FixedConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
info->m_numConstraintRows = 6;
|
||||
info->nub = 6;
|
||||
}
|
||||
|
||||
void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
//fix the 3 linear degrees of freedom
|
||||
|
||||
const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
|
||||
const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
|
||||
const b3Vector3& worldPosB = bodies[m_rbB].m_pos;
|
||||
const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
|
||||
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[info->rowskip + 1] = 1;
|
||||
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
|
||||
|
||||
b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA);
|
||||
{
|
||||
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
|
||||
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
|
||||
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
|
||||
b3Vector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
|
||||
}
|
||||
|
||||
if (info->m_J2linearAxis)
|
||||
{
|
||||
info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[info->rowskip + 1] = -1;
|
||||
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
|
||||
}
|
||||
|
||||
b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB);
|
||||
|
||||
{
|
||||
// b3Vector3 a2n = -a2;
|
||||
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
|
||||
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
|
||||
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
|
||||
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
|
||||
}
|
||||
|
||||
// set right hand side for the linear dofs
|
||||
b3Scalar k = info->fps * info->erp;
|
||||
b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA);
|
||||
int j;
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
info->m_constraintError[j * info->rowskip] = linearError[j];
|
||||
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
|
||||
}
|
||||
|
||||
//fix the 3 angular degrees of freedom
|
||||
|
||||
int start_row = 3;
|
||||
int s = info->rowskip;
|
||||
int start_index = start_row * s;
|
||||
|
||||
// 3 rows to make body rotations equal
|
||||
info->m_J1angularAxis[start_index] = 1;
|
||||
info->m_J1angularAxis[start_index + s + 1] = 1;
|
||||
info->m_J1angularAxis[start_index + s * 2 + 2] = 1;
|
||||
if (info->m_J2angularAxis)
|
||||
{
|
||||
info->m_J2angularAxis[start_index] = -1;
|
||||
info->m_J2angularAxis[start_index + s + 1] = -1;
|
||||
info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
|
||||
}
|
||||
|
||||
// set right hand side for the angular dofs
|
||||
|
||||
b3Vector3 diff;
|
||||
b3Scalar angle;
|
||||
b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse();
|
||||
|
||||
b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle);
|
||||
diff *= -angle;
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
|
||||
}
|
||||
}
|
||||
|
|
@ -1,34 +0,0 @@
|
|||
|
||||
#ifndef B3_FIXED_CONSTRAINT_H
|
||||
#define B3_FIXED_CONSTRAINT_H
|
||||
|
||||
#include "b3TypedConstraint.h"
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3FixedConstraint : public b3TypedConstraint
|
||||
{
|
||||
b3Vector3 m_pivotInA;
|
||||
b3Vector3 m_pivotInB;
|
||||
b3Quaternion m_relTargetAB;
|
||||
|
||||
public:
|
||||
b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB);
|
||||
|
||||
virtual ~b3FixedConstraint();
|
||||
|
||||
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
virtual void setParam(int num, b3Scalar value, int axis = -1)
|
||||
{
|
||||
b3Assert(0);
|
||||
}
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const
|
||||
{
|
||||
b3Assert(0);
|
||||
return 0.f;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_FIXED_CONSTRAINT_H
|
||||
|
|
@ -1,737 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
/*
|
||||
2007-09-09
|
||||
Refactored by Francisco Le?n
|
||||
email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
#include "b3Generic6DofConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
#define D6_USE_OBSOLETE_METHOD false
|
||||
#define D6_USE_FRAME_OFFSET true
|
||||
|
||||
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
|
||||
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
|
||||
{
|
||||
calculateTransforms(bodies);
|
||||
}
|
||||
|
||||
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
||||
|
||||
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
|
||||
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
|
||||
{
|
||||
int i = index % 3;
|
||||
int j = index / 3;
|
||||
return mat[i][j];
|
||||
}
|
||||
|
||||
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
||||
bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
|
||||
bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
|
||||
{
|
||||
// // rot = cy*cz -cy*sz sy
|
||||
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
||||
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
||||
//
|
||||
|
||||
b3Scalar fi = btGetMatrixElem(mat, 2);
|
||||
if (fi < b3Scalar(1.0f))
|
||||
{
|
||||
if (fi > b3Scalar(-1.0f))
|
||||
{
|
||||
xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
|
||||
xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
|
||||
xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
||||
xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
|
||||
xyz[1] = -B3_HALF_PI;
|
||||
xyz[2] = b3Scalar(0.0);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
||||
xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
|
||||
xyz[1] = B3_HALF_PI;
|
||||
xyz[2] = 0.0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
//////////////////////////// b3RotationalLimitMotor ////////////////////////////////////
|
||||
|
||||
int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
|
||||
{
|
||||
if (m_loLimit > m_hiLimit)
|
||||
{
|
||||
m_currentLimit = 0; //Free from violation
|
||||
return 0;
|
||||
}
|
||||
if (test_value < m_loLimit)
|
||||
{
|
||||
m_currentLimit = 1; //low limit violation
|
||||
m_currentLimitError = test_value - m_loLimit;
|
||||
if (m_currentLimitError > B3_PI)
|
||||
m_currentLimitError -= B3_2_PI;
|
||||
else if (m_currentLimitError < -B3_PI)
|
||||
m_currentLimitError += B3_2_PI;
|
||||
return 1;
|
||||
}
|
||||
else if (test_value > m_hiLimit)
|
||||
{
|
||||
m_currentLimit = 2; //High limit violation
|
||||
m_currentLimitError = test_value - m_hiLimit;
|
||||
if (m_currentLimitError > B3_PI)
|
||||
m_currentLimitError -= B3_2_PI;
|
||||
else if (m_currentLimitError < -B3_PI)
|
||||
m_currentLimitError += B3_2_PI;
|
||||
return 2;
|
||||
};
|
||||
|
||||
m_currentLimit = 0; //Free from violation
|
||||
return 0;
|
||||
}
|
||||
|
||||
//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
|
||||
|
||||
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
|
||||
|
||||
int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
|
||||
{
|
||||
b3Scalar loLimit = m_lowerLimit[limitIndex];
|
||||
b3Scalar hiLimit = m_upperLimit[limitIndex];
|
||||
if (loLimit > hiLimit)
|
||||
{
|
||||
m_currentLimit[limitIndex] = 0; //Free from violation
|
||||
m_currentLimitError[limitIndex] = b3Scalar(0.f);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (test_value < loLimit)
|
||||
{
|
||||
m_currentLimit[limitIndex] = 2; //low limit violation
|
||||
m_currentLimitError[limitIndex] = test_value - loLimit;
|
||||
return 2;
|
||||
}
|
||||
else if (test_value > hiLimit)
|
||||
{
|
||||
m_currentLimit[limitIndex] = 1; //High limit violation
|
||||
m_currentLimitError[limitIndex] = test_value - hiLimit;
|
||||
return 1;
|
||||
};
|
||||
|
||||
m_currentLimit[limitIndex] = 0; //Free from violation
|
||||
m_currentLimitError[limitIndex] = b3Scalar(0.f);
|
||||
return 0;
|
||||
}
|
||||
|
||||
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
|
||||
|
||||
void b3Generic6DofConstraint::calculateAngleInfo()
|
||||
{
|
||||
b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
|
||||
matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
|
||||
// in euler angle mode we do not actually constrain the angular velocity
|
||||
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
|
||||
//
|
||||
// to get constrain w2-w1 along ...not
|
||||
// ------ --------------------- ------
|
||||
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
|
||||
// d(angle[1])/dt = 0 ax[1]
|
||||
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
|
||||
//
|
||||
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
|
||||
// to prove the result for angle[0], write the expression for angle[0] from
|
||||
// GetInfo1 then take the derivative. to prove this for angle[2] it is
|
||||
// easier to take the euler rate expression for d(angle[2])/dt with respect
|
||||
// to the components of w and set that to 0.
|
||||
b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
|
||||
b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
|
||||
|
||||
m_calculatedAxis[1] = axis2.cross(axis0);
|
||||
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
|
||||
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
|
||||
|
||||
m_calculatedAxis[0].normalize();
|
||||
m_calculatedAxis[1].normalize();
|
||||
m_calculatedAxis[2].normalize();
|
||||
}
|
||||
|
||||
static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
|
||||
{
|
||||
b3Transform tr(body.m_quat, body.m_pos);
|
||||
return tr;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
|
||||
{
|
||||
b3Transform transA;
|
||||
b3Transform transB;
|
||||
transA = getCenterOfMassTransform(bodies[m_rbA]);
|
||||
transB = getCenterOfMassTransform(bodies[m_rbB]);
|
||||
calculateTransforms(transA, transB, bodies);
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
|
||||
{
|
||||
m_calculatedTransformA = transA * m_frameInA;
|
||||
m_calculatedTransformB = transB * m_frameInB;
|
||||
calculateLinearInfo();
|
||||
calculateAngleInfo();
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{ // get weight factors depending on masses
|
||||
b3Scalar miA = bodies[m_rbA].m_invMass;
|
||||
b3Scalar miB = bodies[m_rbB].m_invMass;
|
||||
m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
|
||||
b3Scalar miS = miA + miB;
|
||||
if (miS > b3Scalar(0.f))
|
||||
{
|
||||
m_factA = miB / miS;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_factA = b3Scalar(0.5f);
|
||||
}
|
||||
m_factB = b3Scalar(1.0f) - m_factA;
|
||||
}
|
||||
}
|
||||
|
||||
bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
|
||||
{
|
||||
b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
|
||||
angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
|
||||
m_angularLimits[axis_index].m_currentPosition = angle;
|
||||
//test limits
|
||||
m_angularLimits[axis_index].testLimitValue(angle);
|
||||
return m_angularLimits[axis_index].needApplyTorques();
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
//prepare constraint
|
||||
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 6;
|
||||
int i;
|
||||
//test linear limits
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if (m_linearLimits.needApplyForce(i))
|
||||
{
|
||||
info->m_numConstraintRows++;
|
||||
info->nub--;
|
||||
}
|
||||
}
|
||||
//test angular limits
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
if (testAngularLimitMotor(i))
|
||||
{
|
||||
info->m_numConstraintRows++;
|
||||
info->nub--;
|
||||
}
|
||||
}
|
||||
// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
//pre-allocate all 6
|
||||
info->m_numConstraintRows = 6;
|
||||
info->nub = 0;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
|
||||
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
|
||||
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
|
||||
const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
|
||||
const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
|
||||
const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
|
||||
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{ // for stability better to solve angular limits first
|
||||
int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
}
|
||||
else
|
||||
{ // leave old version for compatibility
|
||||
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
}
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
|
||||
{
|
||||
//prepare constraint
|
||||
calculateTransforms(transA, transB, bodies);
|
||||
|
||||
int i;
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
testAngularLimitMotor(i);
|
||||
}
|
||||
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{ // for stability better to solve angular limits first
|
||||
int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
}
|
||||
else
|
||||
{ // leave old version for compatibility
|
||||
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
|
||||
}
|
||||
}
|
||||
|
||||
int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
|
||||
{
|
||||
// int row = 0;
|
||||
//solve linear limits
|
||||
b3RotationalLimitMotor limot;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (m_linearLimits.needApplyForce(i))
|
||||
{ // re-use rotational motor code
|
||||
limot.m_bounce = b3Scalar(0.f);
|
||||
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
|
||||
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
|
||||
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
|
||||
limot.m_damping = m_linearLimits.m_damping;
|
||||
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
|
||||
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
|
||||
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
|
||||
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
|
||||
limot.m_maxLimitForce = b3Scalar(0.f);
|
||||
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
|
||||
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
|
||||
b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
|
||||
limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
|
||||
limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{
|
||||
int indx1 = (i + 1) % 3;
|
||||
int indx2 = (i + 2) % 3;
|
||||
int rotAllowed = 1; // rotations around orthos to current axis
|
||||
if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
|
||||
{
|
||||
rotAllowed = 0;
|
||||
}
|
||||
row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
|
||||
}
|
||||
else
|
||||
{
|
||||
row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
return row;
|
||||
}
|
||||
|
||||
int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
|
||||
{
|
||||
b3Generic6DofConstraint* d6constraint = this;
|
||||
int row = row_offset;
|
||||
//solve angular limits
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
|
||||
{
|
||||
b3Vector3 axis = d6constraint->getAxis(i);
|
||||
int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
|
||||
{
|
||||
m_angularLimits[i].m_normalCFM = info->cfm[0];
|
||||
}
|
||||
if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
|
||||
{
|
||||
m_angularLimits[i].m_stopCFM = info->cfm[0];
|
||||
}
|
||||
if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
|
||||
{
|
||||
m_angularLimits[i].m_stopERP = info->erp;
|
||||
}
|
||||
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
|
||||
transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
|
||||
}
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
|
||||
{
|
||||
m_frameInA = frameA;
|
||||
m_frameInB = frameB;
|
||||
|
||||
calculateTransforms(bodies);
|
||||
}
|
||||
|
||||
b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
|
||||
{
|
||||
return m_calculatedAxis[axis_index];
|
||||
}
|
||||
|
||||
b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
|
||||
{
|
||||
return m_calculatedLinearDiff[axisIndex];
|
||||
}
|
||||
|
||||
b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
|
||||
{
|
||||
return m_calculatedAxisAngleDiff[axisIndex];
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
|
||||
{
|
||||
b3Scalar imA = bodies[m_rbA].m_invMass;
|
||||
b3Scalar imB = bodies[m_rbB].m_invMass;
|
||||
b3Scalar weight;
|
||||
if (imB == b3Scalar(0.0))
|
||||
{
|
||||
weight = b3Scalar(1.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
weight = imA / (imA + imB);
|
||||
}
|
||||
const b3Vector3& pA = m_calculatedTransformA.getOrigin();
|
||||
const b3Vector3& pB = m_calculatedTransformB.getOrigin();
|
||||
m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
|
||||
return;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::calculateLinearInfo()
|
||||
{
|
||||
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
|
||||
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
|
||||
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
|
||||
}
|
||||
}
|
||||
|
||||
int b3Generic6DofConstraint::get_limit_motor_info2(
|
||||
b3RotationalLimitMotor* limot,
|
||||
const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
|
||||
b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
|
||||
{
|
||||
int srow = row * info->rowskip;
|
||||
bool powered = limot->m_enableMotor;
|
||||
int limit = limot->m_currentLimit;
|
||||
if (powered || limit)
|
||||
{ // if the joint is powered, or has joint limits, add in the extra row
|
||||
b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
|
||||
b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
|
||||
if (J1)
|
||||
{
|
||||
J1[srow + 0] = ax1[0];
|
||||
J1[srow + 1] = ax1[1];
|
||||
J1[srow + 2] = ax1[2];
|
||||
}
|
||||
if (J2)
|
||||
{
|
||||
J2[srow + 0] = -ax1[0];
|
||||
J2[srow + 1] = -ax1[1];
|
||||
J2[srow + 2] = -ax1[2];
|
||||
}
|
||||
if ((!rotational))
|
||||
{
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{
|
||||
b3Vector3 tmpA, tmpB, relA, relB;
|
||||
// get vector from bodyB to frameB in WCS
|
||||
relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
|
||||
// get its projection to constraint axis
|
||||
b3Vector3 projB = ax1 * relB.dot(ax1);
|
||||
// get vector directed from bodyB to constraint axis (and orthogonal to it)
|
||||
b3Vector3 orthoB = relB - projB;
|
||||
// same for bodyA
|
||||
relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
|
||||
b3Vector3 projA = ax1 * relA.dot(ax1);
|
||||
b3Vector3 orthoA = relA - projA;
|
||||
// get desired offset between frames A and B along constraint axis
|
||||
b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
|
||||
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
|
||||
b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
|
||||
// get offset vectors relA and relB
|
||||
relA = orthoA + totalDist * m_factA;
|
||||
relB = orthoB - totalDist * m_factB;
|
||||
tmpA = relA.cross(ax1);
|
||||
tmpB = relB.cross(ax1);
|
||||
if (m_hasStaticBody && (!rotAllowed))
|
||||
{
|
||||
tmpA *= m_factA;
|
||||
tmpB *= m_factB;
|
||||
}
|
||||
int i;
|
||||
for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
|
||||
for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Vector3 ltd; // Linear Torque Decoupling vector
|
||||
b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
|
||||
ltd = c.cross(ax1);
|
||||
info->m_J1angularAxis[srow + 0] = ltd[0];
|
||||
info->m_J1angularAxis[srow + 1] = ltd[1];
|
||||
info->m_J1angularAxis[srow + 2] = ltd[2];
|
||||
|
||||
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
|
||||
ltd = -c.cross(ax1);
|
||||
info->m_J2angularAxis[srow + 0] = ltd[0];
|
||||
info->m_J2angularAxis[srow + 1] = ltd[1];
|
||||
info->m_J2angularAxis[srow + 2] = ltd[2];
|
||||
}
|
||||
}
|
||||
// if we're limited low and high simultaneously, the joint motor is
|
||||
// ineffective
|
||||
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
|
||||
info->m_constraintError[srow] = b3Scalar(0.f);
|
||||
if (powered)
|
||||
{
|
||||
info->cfm[srow] = limot->m_normalCFM;
|
||||
if (!limit)
|
||||
{
|
||||
b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
|
||||
|
||||
b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
|
||||
limot->m_loLimit,
|
||||
limot->m_hiLimit,
|
||||
tag_vel,
|
||||
info->fps * limot->m_stopERP);
|
||||
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
|
||||
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
|
||||
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
|
||||
}
|
||||
}
|
||||
if (limit)
|
||||
{
|
||||
b3Scalar k = info->fps * limot->m_stopERP;
|
||||
if (!rotational)
|
||||
{
|
||||
info->m_constraintError[srow] += k * limot->m_currentLimitError;
|
||||
}
|
||||
else
|
||||
{
|
||||
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
|
||||
}
|
||||
info->cfm[srow] = limot->m_stopCFM;
|
||||
if (limot->m_loLimit == limot->m_hiLimit)
|
||||
{ // limited low and high simultaneously
|
||||
info->m_lowerLimit[srow] = -B3_INFINITY;
|
||||
info->m_upperLimit[srow] = B3_INFINITY;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (limit == 1)
|
||||
{
|
||||
info->m_lowerLimit[srow] = 0;
|
||||
info->m_upperLimit[srow] = B3_INFINITY;
|
||||
}
|
||||
else
|
||||
{
|
||||
info->m_lowerLimit[srow] = -B3_INFINITY;
|
||||
info->m_upperLimit[srow] = 0;
|
||||
}
|
||||
// deal with bounce
|
||||
if (limot->m_bounce > 0)
|
||||
{
|
||||
// calculate joint velocity
|
||||
b3Scalar vel;
|
||||
if (rotational)
|
||||
{
|
||||
vel = angVelA.dot(ax1);
|
||||
//make sure that if no body -> angVelB == zero vec
|
||||
// if (body1)
|
||||
vel -= angVelB.dot(ax1);
|
||||
}
|
||||
else
|
||||
{
|
||||
vel = linVelA.dot(ax1);
|
||||
//make sure that if no body -> angVelB == zero vec
|
||||
// if (body1)
|
||||
vel -= linVelB.dot(ax1);
|
||||
}
|
||||
// only apply bounce if the velocity is incoming, and if the
|
||||
// resulting c[] exceeds what we already have.
|
||||
if (limit == 1)
|
||||
{
|
||||
if (vel < 0)
|
||||
{
|
||||
b3Scalar newc = -limot->m_bounce * vel;
|
||||
if (newc > info->m_constraintError[srow])
|
||||
info->m_constraintError[srow] = newc;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (vel > 0)
|
||||
{
|
||||
b3Scalar newc = -limot->m_bounce * vel;
|
||||
if (newc < info->m_constraintError[srow])
|
||||
info->m_constraintError[srow] = newc;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
|
||||
{
|
||||
if ((axis >= 0) && (axis < 3))
|
||||
{
|
||||
switch (num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP:
|
||||
m_linearLimits.m_stopERP[axis] = value;
|
||||
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM:
|
||||
m_linearLimits.m_stopCFM[axis] = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM:
|
||||
m_linearLimits.m_normalCFM[axis] = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
default:
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else if ((axis >= 3) && (axis < 6))
|
||||
{
|
||||
switch (num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP:
|
||||
m_angularLimits[axis - 3].m_stopERP = value;
|
||||
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM:
|
||||
m_angularLimits[axis - 3].m_stopCFM = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM:
|
||||
m_angularLimits[axis - 3].m_normalCFM = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
default:
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
|
||||
{
|
||||
b3Scalar retVal = 0;
|
||||
if ((axis >= 0) && (axis < 3))
|
||||
{
|
||||
switch (num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP:
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_stopERP[axis];
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM:
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_stopCFM[axis];
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM:
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_normalCFM[axis];
|
||||
break;
|
||||
default:
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else if ((axis >= 3) && (axis < 6))
|
||||
{
|
||||
switch (num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP:
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_stopERP;
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM:
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_stopCFM;
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM:
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_normalCFM;
|
||||
break;
|
||||
default:
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
|
||||
{
|
||||
b3Vector3 zAxis = axis1.normalized();
|
||||
b3Vector3 yAxis = axis2.normalized();
|
||||
b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
|
||||
b3Transform frameInW;
|
||||
frameInW.setIdentity();
|
||||
frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
|
||||
xAxis[1], yAxis[1], zAxis[1],
|
||||
xAxis[2], yAxis[2], zAxis[2]);
|
||||
|
||||
// now get constraint frame in local coordinate systems
|
||||
m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
|
||||
m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
|
||||
|
||||
calculateTransforms(bodies);
|
||||
}
|
||||
|
|
@ -1,517 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev
|
||||
/// Added support for generic constraint solver through getInfo1/getInfo2 methods
|
||||
|
||||
/*
|
||||
2007-09-09
|
||||
b3Generic6DofConstraint Refactored by Francisco Le?n
|
||||
email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
|
||||
#define B3_GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "b3JacobianEntry.h"
|
||||
#include "b3TypedConstraint.h"
|
||||
|
||||
struct b3RigidBodyData;
|
||||
|
||||
//! Rotation Limit structure for generic joints
|
||||
class b3RotationalLimitMotor
|
||||
{
|
||||
public:
|
||||
//! limit_parameters
|
||||
//!@{
|
||||
b3Scalar m_loLimit; //!< joint limit
|
||||
b3Scalar m_hiLimit; //!< joint limit
|
||||
b3Scalar m_targetVelocity; //!< target motor velocity
|
||||
b3Scalar m_maxMotorForce; //!< max force on motor
|
||||
b3Scalar m_maxLimitForce; //!< max force on limit
|
||||
b3Scalar m_damping; //!< Damping.
|
||||
b3Scalar m_limitSoftness; //! Relaxation factor
|
||||
b3Scalar m_normalCFM; //!< Constraint force mixing factor
|
||||
b3Scalar m_stopERP; //!< Error tolerance factor when joint is at limit
|
||||
b3Scalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
|
||||
b3Scalar m_bounce; //!< restitution factor
|
||||
bool m_enableMotor;
|
||||
|
||||
//!@}
|
||||
|
||||
//! temp_variables
|
||||
//!@{
|
||||
b3Scalar m_currentLimitError; //! How much is violated this limit
|
||||
b3Scalar m_currentPosition; //! current value of angle
|
||||
int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
|
||||
b3Scalar m_accumulatedImpulse;
|
||||
//!@}
|
||||
|
||||
b3RotationalLimitMotor()
|
||||
{
|
||||
m_accumulatedImpulse = 0.f;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 6.0f;
|
||||
m_maxLimitForce = 300.0f;
|
||||
m_loLimit = 1.0f;
|
||||
m_hiLimit = -1.0f;
|
||||
m_normalCFM = 0.f;
|
||||
m_stopERP = 0.2f;
|
||||
m_stopCFM = 0.f;
|
||||
m_bounce = 0.0f;
|
||||
m_damping = 1.0f;
|
||||
m_limitSoftness = 0.5f;
|
||||
m_currentLimit = 0;
|
||||
m_currentLimitError = 0;
|
||||
m_enableMotor = false;
|
||||
}
|
||||
|
||||
b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
|
||||
{
|
||||
m_targetVelocity = limot.m_targetVelocity;
|
||||
m_maxMotorForce = limot.m_maxMotorForce;
|
||||
m_limitSoftness = limot.m_limitSoftness;
|
||||
m_loLimit = limot.m_loLimit;
|
||||
m_hiLimit = limot.m_hiLimit;
|
||||
m_normalCFM = limot.m_normalCFM;
|
||||
m_stopERP = limot.m_stopERP;
|
||||
m_stopCFM = limot.m_stopCFM;
|
||||
m_bounce = limot.m_bounce;
|
||||
m_currentLimit = limot.m_currentLimit;
|
||||
m_currentLimitError = limot.m_currentLimitError;
|
||||
m_enableMotor = limot.m_enableMotor;
|
||||
}
|
||||
|
||||
//! Is limited
|
||||
bool isLimited()
|
||||
{
|
||||
if (m_loLimit > m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! Need apply correction
|
||||
bool needApplyTorques()
|
||||
{
|
||||
if (m_currentLimit == 0 && m_enableMotor == false) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! calculates error
|
||||
/*!
|
||||
calculates m_currentLimit and m_currentLimitError.
|
||||
*/
|
||||
int testLimitValue(b3Scalar test_value);
|
||||
|
||||
//! apply the correction impulses for two bodies
|
||||
b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
|
||||
};
|
||||
|
||||
class b3TranslationalLimitMotor
|
||||
{
|
||||
public:
|
||||
b3Vector3 m_lowerLimit; //!< the constraint lower limits
|
||||
b3Vector3 m_upperLimit; //!< the constraint upper limits
|
||||
b3Vector3 m_accumulatedImpulse;
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
b3Vector3 m_normalCFM; //!< Constraint force mixing factor
|
||||
b3Vector3 m_stopERP; //!< Error tolerance factor when joint is at limit
|
||||
b3Vector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
|
||||
b3Vector3 m_targetVelocity; //!< target motor velocity
|
||||
b3Vector3 m_maxMotorForce; //!< max force on motor
|
||||
b3Vector3 m_currentLimitError; //! How much is violated this limit
|
||||
b3Vector3 m_currentLinearDiff; //! Current relative offset of constraint frames
|
||||
b3Scalar m_limitSoftness; //!< Softness for linear limit
|
||||
b3Scalar m_damping; //!< Damping for linear limit
|
||||
b3Scalar m_restitution; //! Bounce parameter for linear limit
|
||||
//!@}
|
||||
bool m_enableMotor[3];
|
||||
int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
|
||||
|
||||
b3TranslationalLimitMotor()
|
||||
{
|
||||
m_lowerLimit.setValue(0.f, 0.f, 0.f);
|
||||
m_upperLimit.setValue(0.f, 0.f, 0.f);
|
||||
m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
|
||||
m_normalCFM.setValue(0.f, 0.f, 0.f);
|
||||
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
|
||||
m_stopCFM.setValue(0.f, 0.f, 0.f);
|
||||
|
||||
m_limitSoftness = 0.7f;
|
||||
m_damping = b3Scalar(1.0f);
|
||||
m_restitution = b3Scalar(0.5f);
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = false;
|
||||
m_targetVelocity[i] = b3Scalar(0.f);
|
||||
m_maxMotorForce[i] = b3Scalar(0.f);
|
||||
}
|
||||
}
|
||||
|
||||
b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
|
||||
{
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_accumulatedImpulse = other.m_accumulatedImpulse;
|
||||
|
||||
m_limitSoftness = other.m_limitSoftness;
|
||||
m_damping = other.m_damping;
|
||||
m_restitution = other.m_restitution;
|
||||
m_normalCFM = other.m_normalCFM;
|
||||
m_stopERP = other.m_stopERP;
|
||||
m_stopCFM = other.m_stopCFM;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = other.m_enableMotor[i];
|
||||
m_targetVelocity[i] = other.m_targetVelocity[i];
|
||||
m_maxMotorForce[i] = other.m_maxMotorForce[i];
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
inline bool isLimited(int limitIndex)
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
inline bool needApplyForce(int limitIndex)
|
||||
{
|
||||
if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
|
||||
return true;
|
||||
}
|
||||
int testLimitValue(int limitIndex, b3Scalar test_value);
|
||||
|
||||
b3Scalar solveLinearAxis(
|
||||
b3Scalar timeStep,
|
||||
b3Scalar jacDiagABInv,
|
||||
b3RigidBodyData& body1, const b3Vector3& pointInA,
|
||||
b3RigidBodyData& body2, const b3Vector3& pointInB,
|
||||
int limit_index,
|
||||
const b3Vector3& axis_normal_on_a,
|
||||
const b3Vector3& anchorPos);
|
||||
};
|
||||
|
||||
enum b36DofFlags
|
||||
{
|
||||
B3_6DOF_FLAGS_CFM_NORM = 1,
|
||||
B3_6DOF_FLAGS_CFM_STOP = 2,
|
||||
B3_6DOF_FLAGS_ERP_STOP = 4
|
||||
};
|
||||
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
|
||||
|
||||
/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/*!
|
||||
b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
|
||||
currently this limit supports rotational motors<br>
|
||||
<ul>
|
||||
<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method.
|
||||
At this moment translational motors are not supported. May be in the future. </li>
|
||||
|
||||
<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit.
|
||||
This is accessible through b3Generic6DofConstraint.getLimitMotor method,
|
||||
This brings support for limit parameters and motors. </li>
|
||||
|
||||
<li> Angulars limits have these possible ranges:
|
||||
<table border=1 >
|
||||
<tr>
|
||||
<td><b>AXIS</b></td>
|
||||
<td><b>MIN ANGLE</b></td>
|
||||
<td><b>MAX ANGLE</b></td>
|
||||
</tr><tr>
|
||||
<td>X</td>
|
||||
<td>-PI</td>
|
||||
<td>PI</td>
|
||||
</tr><tr>
|
||||
<td>Y</td>
|
||||
<td>-PI/2</td>
|
||||
<td>PI/2</td>
|
||||
</tr><tr>
|
||||
<td>Z</td>
|
||||
<td>-PI</td>
|
||||
<td>PI</td>
|
||||
</tr>
|
||||
</table>
|
||||
</li>
|
||||
</ul>
|
||||
|
||||
*/
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3Generic6DofConstraint : public b3TypedConstraint
|
||||
{
|
||||
protected:
|
||||
//! relative_frames
|
||||
//!@{
|
||||
b3Transform m_frameInA; //!< the constraint space w.r.t body A
|
||||
b3Transform m_frameInB; //!< the constraint space w.r.t body B
|
||||
//!@}
|
||||
|
||||
//! Jacobians
|
||||
//!@{
|
||||
// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
|
||||
// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
|
||||
//!@}
|
||||
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
b3TranslationalLimitMotor m_linearLimits;
|
||||
//!@}
|
||||
|
||||
//! hinge_parameters
|
||||
//!@{
|
||||
b3RotationalLimitMotor m_angularLimits[3];
|
||||
//!@}
|
||||
|
||||
protected:
|
||||
//! temporal variables
|
||||
//!@{
|
||||
b3Transform m_calculatedTransformA;
|
||||
b3Transform m_calculatedTransformB;
|
||||
b3Vector3 m_calculatedAxisAngleDiff;
|
||||
b3Vector3 m_calculatedAxis[3];
|
||||
b3Vector3 m_calculatedLinearDiff;
|
||||
b3Scalar m_timeStep;
|
||||
b3Scalar m_factA;
|
||||
b3Scalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
|
||||
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
||||
|
||||
bool m_useLinearReferenceFrameA;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
|
||||
int m_flags;
|
||||
|
||||
//!@}
|
||||
|
||||
b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
|
||||
{
|
||||
b3Assert(0);
|
||||
(void)other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
|
||||
|
||||
int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
|
||||
|
||||
// tests linear limits
|
||||
void calculateLinearInfo();
|
||||
|
||||
//! calcs the euler angles between the two bodies.
|
||||
void calculateAngleInfo();
|
||||
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
|
||||
|
||||
//! Calcs global transform of the offsets
|
||||
/*!
|
||||
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
|
||||
\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
|
||||
*/
|
||||
void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
|
||||
|
||||
void calculateTransforms(const b3RigidBodyData* bodies);
|
||||
|
||||
//! Gets the global transform of the offset for body A
|
||||
/*!
|
||||
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const b3Transform& getCalculatedTransformA() const
|
||||
{
|
||||
return m_calculatedTransformA;
|
||||
}
|
||||
|
||||
//! Gets the global transform of the offset for body B
|
||||
/*!
|
||||
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const b3Transform& getCalculatedTransformB() const
|
||||
{
|
||||
return m_calculatedTransformB;
|
||||
}
|
||||
|
||||
const b3Transform& getFrameOffsetA() const
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
const b3Transform& getFrameOffsetB() const
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
b3Transform& getFrameOffsetA()
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
b3Transform& getFrameOffsetB()
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
|
||||
|
||||
void updateRHS(b3Scalar timeStep);
|
||||
|
||||
//! Get the rotation axis in global coordinates
|
||||
b3Vector3 getAxis(int axis_index) const;
|
||||
|
||||
//! Get the relative Euler angle
|
||||
/*!
|
||||
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
b3Scalar getAngle(int axis_index) const;
|
||||
|
||||
//! Get the relative position of the constraint pivot
|
||||
/*!
|
||||
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
b3Scalar getRelativePivotPosition(int axis_index) const;
|
||||
|
||||
void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
|
||||
|
||||
//! Test angular limit.
|
||||
/*!
|
||||
Calculates angular correction and returns true if limit needs to be corrected.
|
||||
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
bool testAngularLimitMotor(int axis_index);
|
||||
|
||||
void setLinearLowerLimit(const b3Vector3& linearLower)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit = linearLower;
|
||||
}
|
||||
|
||||
void getLinearLowerLimit(b3Vector3 & linearLower)
|
||||
{
|
||||
linearLower = m_linearLimits.m_lowerLimit;
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const b3Vector3& linearUpper)
|
||||
{
|
||||
m_linearLimits.m_upperLimit = linearUpper;
|
||||
}
|
||||
|
||||
void getLinearUpperLimit(b3Vector3 & linearUpper)
|
||||
{
|
||||
linearUpper = m_linearLimits.m_upperLimit;
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const b3Vector3& angularLower)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
|
||||
}
|
||||
|
||||
void getAngularLowerLimit(b3Vector3 & angularLower)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularLower[i] = m_angularLimits[i].m_loLimit;
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const b3Vector3& angularUpper)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
|
||||
}
|
||||
|
||||
void getAngularUpperLimit(b3Vector3 & angularUpper)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
angularUpper[i] = m_angularLimits[i].m_hiLimit;
|
||||
}
|
||||
|
||||
//! Retrieves the angular limit informacion
|
||||
b3RotationalLimitMotor* getRotationalLimitMotor(int index)
|
||||
{
|
||||
return &m_angularLimits[index];
|
||||
}
|
||||
|
||||
//! Retrieves the limit informacion
|
||||
b3TranslationalLimitMotor* getTranslationalLimitMotor()
|
||||
{
|
||||
return &m_linearLimits;
|
||||
}
|
||||
|
||||
//first 3 are linear, next 3 are angular
|
||||
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
|
||||
{
|
||||
if (axis < 3)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit[axis] = lo;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
}
|
||||
else
|
||||
{
|
||||
lo = b3NormalizeAngle(lo);
|
||||
hi = b3NormalizeAngle(hi);
|
||||
m_angularLimits[axis - 3].m_loLimit = lo;
|
||||
m_angularLimits[axis - 3].m_hiLimit = hi;
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
bool isLimited(int limitIndex)
|
||||
{
|
||||
if (limitIndex < 3)
|
||||
{
|
||||
return m_linearLimits.isLimited(limitIndex);
|
||||
}
|
||||
return m_angularLimits[limitIndex - 3].isLimited();
|
||||
}
|
||||
|
||||
virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
|
||||
|
||||
int get_limit_motor_info2(b3RotationalLimitMotor * limot,
|
||||
const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
|
||||
b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
|
||||
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, b3Scalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const;
|
||||
|
||||
void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
|
||||
};
|
||||
|
||||
#endif //B3_GENERIC_6DOF_CONSTRAINT_H
|
||||
|
|
@ -1,150 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_JACOBIAN_ENTRY_H
|
||||
#define B3_JACOBIAN_ENTRY_H
|
||||
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
|
||||
//notes:
|
||||
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
|
||||
// which makes the b3JacobianEntry memory layout 16 bytes
|
||||
// if you only are interested in angular part, just feed massInvA and massInvB zero
|
||||
|
||||
/// Jacobian entry is an abstraction that allows to describe constraints
|
||||
/// it can be used in combination with a constraint solver
|
||||
/// Can be used to relate the effect of an impulse to the constraint error
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3JacobianEntry
|
||||
{
|
||||
public:
|
||||
b3JacobianEntry(){};
|
||||
//constraint between two different rigidbodies
|
||||
b3JacobianEntry(
|
||||
const b3Matrix3x3& world2A,
|
||||
const b3Matrix3x3& world2B,
|
||||
const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
|
||||
const b3Vector3& jointAxis,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Scalar massInvA,
|
||||
const b3Vector3& inertiaInvB,
|
||||
const b3Scalar massInvB)
|
||||
: m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
|
||||
m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
b3JacobianEntry(const b3Vector3& jointAxis,
|
||||
const b3Matrix3x3& world2A,
|
||||
const b3Matrix3x3& world2B,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Vector3& inertiaInvB)
|
||||
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)))
|
||||
{
|
||||
m_aJ = world2A * jointAxis;
|
||||
m_bJ = world2B * -jointAxis;
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
b3JacobianEntry(const b3Vector3& axisInA,
|
||||
const b3Vector3& axisInB,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Vector3& inertiaInvB)
|
||||
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
|
||||
{
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
b3JacobianEntry(
|
||||
const b3Matrix3x3& world2A,
|
||||
const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
|
||||
const b3Vector3& jointAxis,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Scalar massInvA)
|
||||
: m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A * (rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A * (rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
b3Scalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
|
||||
{
|
||||
const b3JacobianEntry& jacA = *this;
|
||||
b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
|
||||
b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
|
||||
return lin + ang;
|
||||
}
|
||||
|
||||
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
|
||||
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
|
||||
{
|
||||
const b3JacobianEntry& jacA = *this;
|
||||
b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
b3Vector3 lin0 = massInvA * lin;
|
||||
b3Vector3 lin1 = massInvB * lin;
|
||||
b3Vector3 sum = ang0 + ang1 + lin0 + lin1;
|
||||
return sum[0] + sum[1] + sum[2];
|
||||
}
|
||||
|
||||
b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB)
|
||||
{
|
||||
b3Vector3 linrel = linvelA - linvelB;
|
||||
b3Vector3 angvela = angvelA * m_aJ;
|
||||
b3Vector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
|
||||
return rel_vel2 + B3_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
b3Vector3 m_linearJointAxis;
|
||||
b3Vector3 m_aJ;
|
||||
b3Vector3 m_bJ;
|
||||
b3Vector3 m_0MinvJt;
|
||||
b3Vector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
b3Scalar m_Adiag;
|
||||
};
|
||||
|
||||
#endif //B3_JACOBIAN_ENTRY_H
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,133 +0,0 @@
|
|||
#ifndef B3_PGS_JACOBI_SOLVER
|
||||
#define B3_PGS_JACOBI_SOLVER
|
||||
|
||||
struct b3Contact4;
|
||||
struct b3ContactPoint;
|
||||
|
||||
class b3Dispatcher;
|
||||
|
||||
#include "b3TypedConstraint.h"
|
||||
#include "b3ContactSolverInfo.h"
|
||||
#include "b3SolverBody.h"
|
||||
#include "b3SolverConstraint.h"
|
||||
|
||||
struct b3RigidBodyData;
|
||||
struct b3InertiaData;
|
||||
|
||||
class b3PgsJacobiSolver
|
||||
{
|
||||
protected:
|
||||
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
|
||||
b3ConstraintArray m_tmpSolverContactConstraintPool;
|
||||
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
|
||||
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||||
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
|
||||
|
||||
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
|
||||
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
|
||||
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||||
b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
|
||||
|
||||
b3AlignedObjectArray<int> m_bodyCount;
|
||||
b3AlignedObjectArray<int> m_bodyCountCheck;
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
|
||||
b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
|
||||
|
||||
bool m_usePgs;
|
||||
void averageVelocities();
|
||||
|
||||
int m_maxOverrideNumSolverIterations;
|
||||
|
||||
int m_numSplitImpulseRecoveries;
|
||||
|
||||
b3Scalar getContactProcessingThreshold(b3Contact4* contact)
|
||||
{
|
||||
return 0.02f;
|
||||
}
|
||||
void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
|
||||
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
|
||||
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
|
||||
b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
|
||||
|
||||
void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
|
||||
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
|
||||
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
|
||||
b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
|
||||
|
||||
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
|
||||
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
|
||||
|
||||
void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
|
||||
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
|
||||
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
|
||||
b3Vector3& rel_pos1, b3Vector3& rel_pos2);
|
||||
|
||||
void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
|
||||
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
|
||||
|
||||
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
void resolveSplitPenetrationSIMD(
|
||||
b3SolverBody& bodyA, b3SolverBody& bodyB,
|
||||
const b3SolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSplitPenetrationImpulseCacheFriendly(
|
||||
b3SolverBody& bodyA, b3SolverBody& bodyB,
|
||||
const b3SolverConstraint& contactConstraint);
|
||||
|
||||
//internal method
|
||||
int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
|
||||
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
|
||||
|
||||
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
|
||||
|
||||
protected:
|
||||
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
|
||||
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3PgsJacobiSolver(bool usePgs);
|
||||
virtual ~b3PgsJacobiSolver();
|
||||
|
||||
// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
|
||||
void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
|
||||
|
||||
b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset();
|
||||
|
||||
unsigned long b3Rand2();
|
||||
|
||||
int b3RandInt2(int n);
|
||||
|
||||
void setRandSeed(unsigned long seed)
|
||||
{
|
||||
m_btSeed2 = seed;
|
||||
}
|
||||
unsigned long getRandSeed() const
|
||||
{
|
||||
return m_btSeed2;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_PGS_JACOBI_SOLVER
|
||||
|
|
@ -1,190 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b3Point2PointConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
#include <new>
|
||||
|
||||
b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
|
||||
: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA)
|
||||
:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
{
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
getInfo1NonVirtual(info, bodies);
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
info->m_numConstraintRows = 3;
|
||||
info->nub = 3;
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
|
||||
{
|
||||
b3Transform trA;
|
||||
trA.setIdentity();
|
||||
trA.setOrigin(bodies[m_rbA].m_pos);
|
||||
trA.setRotation(bodies[m_rbA].m_quat);
|
||||
|
||||
b3Transform trB;
|
||||
trB.setIdentity();
|
||||
trB.setOrigin(bodies[m_rbB].m_pos);
|
||||
trB.setRotation(bodies[m_rbB].m_quat);
|
||||
|
||||
getInfo2NonVirtual(info, trA, trB);
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
|
||||
{
|
||||
//retrieve matrices
|
||||
|
||||
// anchor points in global coordinates with respect to body PORs.
|
||||
|
||||
// set jacobian
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[info->rowskip + 1] = 1;
|
||||
info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
|
||||
|
||||
b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
|
||||
//b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
|
||||
|
||||
{
|
||||
b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
|
||||
b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
|
||||
b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
|
||||
b3Vector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
|
||||
}
|
||||
|
||||
if (info->m_J2linearAxis)
|
||||
{
|
||||
info->m_J2linearAxis[0] = -1;
|
||||
info->m_J2linearAxis[info->rowskip + 1] = -1;
|
||||
info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
|
||||
}
|
||||
|
||||
b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
|
||||
|
||||
{
|
||||
// b3Vector3 a2n = -a2;
|
||||
b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
|
||||
b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
|
||||
b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
|
||||
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
|
||||
}
|
||||
|
||||
// set right hand side
|
||||
b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
|
||||
b3Scalar k = info->fps * currERP;
|
||||
int j;
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
|
||||
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
|
||||
}
|
||||
if (m_flags & B3_P2P_FLAGS_CFM)
|
||||
{
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
info->cfm[j * info->rowskip] = m_cfm;
|
||||
}
|
||||
}
|
||||
|
||||
b3Scalar impulseClamp = m_setting.m_impulseClamp; //
|
||||
for (j = 0; j < 3; j++)
|
||||
{
|
||||
if (m_setting.m_impulseClamp > 0)
|
||||
{
|
||||
info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
|
||||
info->m_upperLimit[j * info->rowskip] = impulseClamp;
|
||||
}
|
||||
}
|
||||
info->m_damping = m_setting.m_damping;
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
|
||||
{
|
||||
if (axis != -1)
|
||||
{
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (num)
|
||||
{
|
||||
case B3_CONSTRAINT_ERP:
|
||||
case B3_CONSTRAINT_STOP_ERP:
|
||||
m_erp = value;
|
||||
m_flags |= B3_P2P_FLAGS_ERP;
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM:
|
||||
case B3_CONSTRAINT_STOP_CFM:
|
||||
m_cfm = value;
|
||||
m_flags |= B3_P2P_FLAGS_CFM;
|
||||
break;
|
||||
default:
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
|
||||
{
|
||||
b3Scalar retVal(B3_INFINITY);
|
||||
if (axis != -1)
|
||||
{
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (num)
|
||||
{
|
||||
case B3_CONSTRAINT_ERP:
|
||||
case B3_CONSTRAINT_STOP_ERP:
|
||||
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
|
||||
retVal = m_erp;
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM:
|
||||
case B3_CONSTRAINT_STOP_CFM:
|
||||
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
|
||||
retVal = m_cfm;
|
||||
break;
|
||||
default:
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
|
@ -1,153 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_POINT2POINTCONSTRAINT_H
|
||||
#define B3_POINT2POINTCONSTRAINT_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
//#include "b3JacobianEntry.h"
|
||||
#include "b3TypedConstraint.h"
|
||||
|
||||
class b3RigidBody;
|
||||
|
||||
#ifdef B3_USE_DOUBLE_PRECISION
|
||||
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
|
||||
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
|
||||
#else
|
||||
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
|
||||
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
|
||||
#endif //B3_USE_DOUBLE_PRECISION
|
||||
|
||||
struct b3ConstraintSetting
|
||||
{
|
||||
b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
|
||||
m_damping(b3Scalar(1.)),
|
||||
m_impulseClamp(b3Scalar(0.))
|
||||
{
|
||||
}
|
||||
b3Scalar m_tau;
|
||||
b3Scalar m_damping;
|
||||
b3Scalar m_impulseClamp;
|
||||
};
|
||||
|
||||
enum b3Point2PointFlags
|
||||
{
|
||||
B3_P2P_FLAGS_ERP = 1,
|
||||
B3_P2P_FLAGS_CFM = 2
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3Point2PointConstraint : public b3TypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
|
||||
b3Vector3 m_pivotInA;
|
||||
b3Vector3 m_pivotInB;
|
||||
|
||||
int m_flags;
|
||||
b3Scalar m_erp;
|
||||
b3Scalar m_cfm;
|
||||
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3ConstraintSetting m_setting;
|
||||
|
||||
b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB);
|
||||
|
||||
//b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
|
||||
|
||||
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
|
||||
|
||||
void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans);
|
||||
|
||||
void updateRHS(b3Scalar timeStep);
|
||||
|
||||
void setPivotA(const b3Vector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void setPivotB(const b3Vector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
|
||||
const b3Vector3& getPivotInA() const
|
||||
{
|
||||
return m_pivotInA;
|
||||
}
|
||||
|
||||
const b3Vector3& getPivotInB() const
|
||||
{
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, b3Scalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const;
|
||||
|
||||
// virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct b3Point2PointConstraintFloatData
|
||||
{
|
||||
b3TypedConstraintData m_typeConstraintData;
|
||||
b3Vector3FloatData m_pivotInA;
|
||||
b3Vector3FloatData m_pivotInB;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct b3Point2PointConstraintDoubleData
|
||||
{
|
||||
b3TypedConstraintData m_typeConstraintData;
|
||||
b3Vector3DoubleData m_pivotInA;
|
||||
b3Vector3DoubleData m_pivotInB;
|
||||
};
|
||||
|
||||
/*
|
||||
B3_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(b3Point2PointConstraintData);
|
||||
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
|
||||
{
|
||||
b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer;
|
||||
|
||||
b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
|
||||
m_pivotInA.serialize(p2pData->m_pivotInA);
|
||||
m_pivotInB.serialize(p2pData->m_pivotInB);
|
||||
|
||||
return b3Point2PointConstraintDataName;
|
||||
}
|
||||
*/
|
||||
|
||||
#endif //B3_POINT2POINTCONSTRAINT_H
|
||||
|
|
@ -1,281 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_SOLVER_BODY_H
|
||||
#define B3_SOLVER_BODY_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
|
||||
#include "Bullet3Common/b3AlignedAllocator.h"
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
|
||||
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
|
||||
#ifdef B3_USE_SSE
|
||||
#define USE_SIMD 1
|
||||
#endif //
|
||||
|
||||
#ifdef USE_SIMD
|
||||
|
||||
struct b3SimdScalar
|
||||
{
|
||||
B3_FORCE_INLINE b3SimdScalar()
|
||||
{
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3SimdScalar(float fl)
|
||||
: m_vec128(_mm_set1_ps(fl))
|
||||
{
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE b3SimdScalar(__m128 v128)
|
||||
: m_vec128(v128)
|
||||
{
|
||||
}
|
||||
union {
|
||||
__m128 m_vec128;
|
||||
float m_floats[4];
|
||||
float x, y, z, w;
|
||||
int m_ints[4];
|
||||
b3Scalar m_unusedPadding;
|
||||
};
|
||||
B3_FORCE_INLINE __m128 get128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE const __m128 get128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void set128(__m128 v128)
|
||||
{
|
||||
m_vec128 = v128;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE operator __m128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
B3_FORCE_INLINE operator const __m128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE operator float() const
|
||||
{
|
||||
return m_floats[0];
|
||||
}
|
||||
};
|
||||
|
||||
///@brief Return the elementwise product of two b3SimdScalar
|
||||
B3_FORCE_INLINE b3SimdScalar
|
||||
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
|
||||
{
|
||||
return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
|
||||
}
|
||||
|
||||
///@brief Return the elementwise product of two b3SimdScalar
|
||||
B3_FORCE_INLINE b3SimdScalar
|
||||
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
|
||||
{
|
||||
return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
|
||||
}
|
||||
|
||||
#else
|
||||
#define b3SimdScalar b3Scalar
|
||||
#endif
|
||||
|
||||
///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3SolverBody
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
b3Transform m_worldTransform;
|
||||
b3Vector3 m_deltaLinearVelocity;
|
||||
b3Vector3 m_deltaAngularVelocity;
|
||||
b3Vector3 m_angularFactor;
|
||||
b3Vector3 m_linearFactor;
|
||||
b3Vector3 m_invMass;
|
||||
b3Vector3 m_pushVelocity;
|
||||
b3Vector3 m_turnVelocity;
|
||||
b3Vector3 m_linearVelocity;
|
||||
b3Vector3 m_angularVelocity;
|
||||
|
||||
union {
|
||||
void* m_originalBody;
|
||||
int m_originalBodyIndex;
|
||||
};
|
||||
|
||||
int padding[3];
|
||||
|
||||
void setWorldTransform(const b3Transform& worldTransform)
|
||||
{
|
||||
m_worldTransform = worldTransform;
|
||||
}
|
||||
|
||||
const b3Transform& getWorldTransform() const
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
|
||||
else
|
||||
velocity.setValue(0, 0, 0);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
angVel = m_angularVelocity + m_deltaAngularVelocity;
|
||||
else
|
||||
angVel.setValue(0, 0, 0);
|
||||
}
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
|
||||
m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
const b3Vector3& getDeltaLinearVelocity() const
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
const b3Vector3& getDeltaAngularVelocity() const
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const b3Vector3& getPushVelocity() const
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
const b3Vector3& getTurnVelocity() const
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////
|
||||
///some internal methods, don't use them
|
||||
|
||||
b3Vector3& internalGetDeltaLinearVelocity()
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
b3Vector3& internalGetDeltaAngularVelocity()
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const b3Vector3& internalGetAngularFactor() const
|
||||
{
|
||||
return m_angularFactor;
|
||||
}
|
||||
|
||||
const b3Vector3& internalGetInvMass() const
|
||||
{
|
||||
return m_invMass;
|
||||
}
|
||||
|
||||
void internalSetInvMass(const b3Vector3& invMass)
|
||||
{
|
||||
m_invMass = invMass;
|
||||
}
|
||||
|
||||
b3Vector3& internalGetPushVelocity()
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
b3Vector3& internalGetTurnVelocity()
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
|
||||
{
|
||||
velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
|
||||
{
|
||||
angVel = m_angularVelocity + m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
|
||||
{
|
||||
//if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
//if (m_originalBody>=0)
|
||||
{
|
||||
m_linearVelocity += m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
|
||||
{
|
||||
(void)timeStep;
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_linearVelocity += m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
b3Transform newTransform;
|
||||
if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
|
||||
{
|
||||
// b3Quaternion orn = m_worldTransform.getRotation();
|
||||
b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
|
||||
m_worldTransform = newTransform;
|
||||
}
|
||||
//m_worldTransform.setRotation(orn);
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_SOLVER_BODY_H
|
||||
|
|
@ -1,73 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_SOLVER_CONSTRAINT_H
|
||||
#define B3_SOLVER_CONSTRAINT_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
//#include "b3JacobianEntry.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
//#define NO_FRICTION_TANGENTIALS 1
|
||||
#include "b3SolverBody.h"
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3SolverConstraint
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
b3Vector3 m_relpos1CrossNormal;
|
||||
b3Vector3 m_contactNormal;
|
||||
|
||||
b3Vector3 m_relpos2CrossNormal;
|
||||
//b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
|
||||
|
||||
b3Vector3 m_angularComponentA;
|
||||
b3Vector3 m_angularComponentB;
|
||||
|
||||
mutable b3SimdScalar m_appliedPushImpulse;
|
||||
mutable b3SimdScalar m_appliedImpulse;
|
||||
int m_padding1;
|
||||
int m_padding2;
|
||||
b3Scalar m_friction;
|
||||
b3Scalar m_jacDiagABInv;
|
||||
b3Scalar m_rhs;
|
||||
b3Scalar m_cfm;
|
||||
|
||||
b3Scalar m_lowerLimit;
|
||||
b3Scalar m_upperLimit;
|
||||
b3Scalar m_rhsPenetration;
|
||||
union {
|
||||
void* m_originalContactPoint;
|
||||
b3Scalar m_unusedPadding4;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
int m_solverBodyIdA;
|
||||
int m_solverBodyIdB;
|
||||
|
||||
enum b3SolverConstraintType
|
||||
{
|
||||
B3_SOLVER_CONTACT_1D = 0,
|
||||
B3_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
|
||||
|
||||
#endif //B3_SOLVER_CONSTRAINT_H
|
||||
|
|
@ -1,151 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b3TypedConstraint.h"
|
||||
//#include "Bullet3Common/b3Serializer.h"
|
||||
|
||||
#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
|
||||
|
||||
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
|
||||
: b3TypedObject(type),
|
||||
m_userConstraintType(-1),
|
||||
m_userConstraintPtr((void*)-1),
|
||||
m_breakingImpulseThreshold(B3_INFINITY),
|
||||
m_isEnabled(true),
|
||||
m_needsFeedback(false),
|
||||
m_overrideNumSolverIterations(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(rbB),
|
||||
m_appliedImpulse(b3Scalar(0.)),
|
||||
m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
|
||||
m_jointFeedback(0)
|
||||
{
|
||||
}
|
||||
|
||||
b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
|
||||
{
|
||||
if (lowLim > uppLim)
|
||||
{
|
||||
return b3Scalar(1.0f);
|
||||
}
|
||||
else if (lowLim == uppLim)
|
||||
{
|
||||
return b3Scalar(0.0f);
|
||||
}
|
||||
b3Scalar lim_fact = b3Scalar(1.0f);
|
||||
b3Scalar delta_max = vel / timeFact;
|
||||
if (delta_max < b3Scalar(0.0f))
|
||||
{
|
||||
if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
|
||||
{
|
||||
lim_fact = (lowLim - pos) / delta_max;
|
||||
}
|
||||
else if (pos < lowLim)
|
||||
{
|
||||
lim_fact = b3Scalar(0.0f);
|
||||
}
|
||||
else
|
||||
{
|
||||
lim_fact = b3Scalar(1.0f);
|
||||
}
|
||||
}
|
||||
else if (delta_max > b3Scalar(0.0f))
|
||||
{
|
||||
if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
|
||||
{
|
||||
lim_fact = (uppLim - pos) / delta_max;
|
||||
}
|
||||
else if (pos > uppLim)
|
||||
{
|
||||
lim_fact = b3Scalar(0.0f);
|
||||
}
|
||||
else
|
||||
{
|
||||
lim_fact = b3Scalar(1.0f);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
lim_fact = b3Scalar(0.0f);
|
||||
}
|
||||
return lim_fact;
|
||||
}
|
||||
|
||||
void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
|
||||
{
|
||||
m_halfRange = (high - low) / 2.0f;
|
||||
m_center = b3NormalizeAngle(low + m_halfRange);
|
||||
m_softness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
}
|
||||
|
||||
void b3AngularLimit::test(const b3Scalar angle)
|
||||
{
|
||||
m_correction = 0.0f;
|
||||
m_sign = 0.0f;
|
||||
m_solveLimit = false;
|
||||
|
||||
if (m_halfRange >= 0.0f)
|
||||
{
|
||||
b3Scalar deviation = b3NormalizeAngle(angle - m_center);
|
||||
if (deviation < -m_halfRange)
|
||||
{
|
||||
m_solveLimit = true;
|
||||
m_correction = -(deviation + m_halfRange);
|
||||
m_sign = +1.0f;
|
||||
}
|
||||
else if (deviation > m_halfRange)
|
||||
{
|
||||
m_solveLimit = true;
|
||||
m_correction = m_halfRange - deviation;
|
||||
m_sign = -1.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b3Scalar b3AngularLimit::getError() const
|
||||
{
|
||||
return m_correction * m_sign;
|
||||
}
|
||||
|
||||
void b3AngularLimit::fit(b3Scalar& angle) const
|
||||
{
|
||||
if (m_halfRange > 0.0f)
|
||||
{
|
||||
b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center);
|
||||
if (!b3Equal(relativeAngle, m_halfRange))
|
||||
{
|
||||
if (relativeAngle > 0.0f)
|
||||
{
|
||||
angle = getHigh();
|
||||
}
|
||||
else
|
||||
{
|
||||
angle = getLow();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b3Scalar b3AngularLimit::getLow() const
|
||||
{
|
||||
return b3NormalizeAngle(m_center - m_halfRange);
|
||||
}
|
||||
|
||||
b3Scalar b3AngularLimit::getHigh() const
|
||||
{
|
||||
return b3NormalizeAngle(m_center + m_halfRange);
|
||||
}
|
||||
|
|
@ -1,469 +0,0 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2010 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_TYPED_CONSTRAINT_H
|
||||
#define B3_TYPED_CONSTRAINT_H
|
||||
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
#include "b3SolverConstraint.h"
|
||||
|
||||
class b3Serializer;
|
||||
|
||||
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
|
||||
enum b3TypedConstraintType
|
||||
{
|
||||
B3_POINT2POINT_CONSTRAINT_TYPE = 3,
|
||||
B3_HINGE_CONSTRAINT_TYPE,
|
||||
B3_CONETWIST_CONSTRAINT_TYPE,
|
||||
B3_D6_CONSTRAINT_TYPE,
|
||||
B3_SLIDER_CONSTRAINT_TYPE,
|
||||
B3_CONTACT_CONSTRAINT_TYPE,
|
||||
B3_D6_SPRING_CONSTRAINT_TYPE,
|
||||
B3_GEAR_CONSTRAINT_TYPE,
|
||||
B3_FIXED_CONSTRAINT_TYPE,
|
||||
B3_MAX_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
enum b3ConstraintParams
|
||||
{
|
||||
B3_CONSTRAINT_ERP = 1,
|
||||
B3_CONSTRAINT_STOP_ERP,
|
||||
B3_CONSTRAINT_CFM,
|
||||
B3_CONSTRAINT_STOP_CFM
|
||||
};
|
||||
|
||||
#if 1
|
||||
#define b3AssertConstrParams(_par) b3Assert(_par)
|
||||
#else
|
||||
#define b3AssertConstrParams(_par)
|
||||
#endif
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct)
|
||||
b3JointFeedback
|
||||
{
|
||||
b3Vector3 m_appliedForceBodyA;
|
||||
b3Vector3 m_appliedTorqueBodyA;
|
||||
b3Vector3 m_appliedForceBodyB;
|
||||
b3Vector3 m_appliedTorqueBodyB;
|
||||
};
|
||||
|
||||
struct b3RigidBodyData;
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
B3_ATTRIBUTE_ALIGNED16(class)
|
||||
b3TypedConstraint : public b3TypedObject
|
||||
{
|
||||
int m_userConstraintType;
|
||||
|
||||
union {
|
||||
int m_userConstraintId;
|
||||
void* m_userConstraintPtr;
|
||||
};
|
||||
|
||||
b3Scalar m_breakingImpulseThreshold;
|
||||
bool m_isEnabled;
|
||||
bool m_needsFeedback;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
b3TypedConstraint& operator=(b3TypedConstraint& other)
|
||||
{
|
||||
b3Assert(0);
|
||||
(void)other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
int m_rbA;
|
||||
int m_rbB;
|
||||
b3Scalar m_appliedImpulse;
|
||||
b3Scalar m_dbgDrawSize;
|
||||
b3JointFeedback* m_jointFeedback;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
|
||||
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
virtual ~b3TypedConstraint(){};
|
||||
b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
|
||||
|
||||
struct b3ConstraintInfo1
|
||||
{
|
||||
int m_numConstraintRows, nub;
|
||||
};
|
||||
|
||||
struct b3ConstraintInfo2
|
||||
{
|
||||
// integrator parameters: frames per second (1/stepsize), default error
|
||||
// reduction parameter (0..1).
|
||||
b3Scalar fps, erp;
|
||||
|
||||
// for the first and second body, pointers to two (linear and angular)
|
||||
// n*3 jacobian sub matrices, stored by rows. these matrices will have
|
||||
// been initialized to 0 on entry. if the second body is zero then the
|
||||
// J2xx pointers may be 0.
|
||||
b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
|
||||
|
||||
// elements to jump from one row to the next in J's
|
||||
int rowskip;
|
||||
|
||||
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
|
||||
// "constraint force mixing" vector. c is set to zero on entry, cfm is
|
||||
// set to a constant value (typically very small or zero) value on entry.
|
||||
b3Scalar *m_constraintError, *cfm;
|
||||
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
b3Scalar *m_lowerLimit, *m_upperLimit;
|
||||
|
||||
// findex vector for variables. see the LCP solver interface for a
|
||||
// description of what this does. this is set to -1 on entry.
|
||||
// note that the returned indexes are relative to the first index of
|
||||
// the constraint.
|
||||
int* findex;
|
||||
// number of solver iterations
|
||||
int m_numIterations;
|
||||
|
||||
//damping of the velocity
|
||||
b3Scalar m_damping;
|
||||
};
|
||||
|
||||
int getOverrideNumSolverIterations() const
|
||||
{
|
||||
return m_overrideNumSolverIterations;
|
||||
}
|
||||
|
||||
///override the number of constraint solver iterations used to solve this constraint
|
||||
///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
|
||||
void setOverrideNumSolverIterations(int overideNumIterations)
|
||||
{
|
||||
m_overrideNumSolverIterations = overideNumIterations;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
|
||||
{
|
||||
(void)ca;
|
||||
(void)solverBodyA;
|
||||
(void)solverBodyB;
|
||||
(void)timeStep;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
|
||||
{
|
||||
m_appliedImpulse = appliedImpulse;
|
||||
}
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
b3Scalar internalGetAppliedImpulse()
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
b3Scalar getBreakingImpulseThreshold() const
|
||||
{
|
||||
return m_breakingImpulseThreshold;
|
||||
}
|
||||
|
||||
void setBreakingImpulseThreshold(b3Scalar threshold)
|
||||
{
|
||||
m_breakingImpulseThreshold = threshold;
|
||||
}
|
||||
|
||||
bool isEnabled() const
|
||||
{
|
||||
return m_isEnabled;
|
||||
}
|
||||
|
||||
void setEnabled(bool enabled)
|
||||
{
|
||||
m_isEnabled = enabled;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
|
||||
|
||||
int getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
int getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
int getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
int getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
int getUserConstraintType() const
|
||||
{
|
||||
return m_userConstraintType;
|
||||
}
|
||||
|
||||
void setUserConstraintType(int userConstraintType)
|
||||
{
|
||||
m_userConstraintType = userConstraintType;
|
||||
};
|
||||
|
||||
void setUserConstraintId(int uid)
|
||||
{
|
||||
m_userConstraintId = uid;
|
||||
}
|
||||
|
||||
int getUserConstraintId() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
void setUserConstraintPtr(void* ptr)
|
||||
{
|
||||
m_userConstraintPtr = ptr;
|
||||
}
|
||||
|
||||
void* getUserConstraintPtr()
|
||||
{
|
||||
return m_userConstraintPtr;
|
||||
}
|
||||
|
||||
void setJointFeedback(b3JointFeedback * jointFeedback)
|
||||
{
|
||||
m_jointFeedback = jointFeedback;
|
||||
}
|
||||
|
||||
const b3JointFeedback* getJointFeedback() const
|
||||
{
|
||||
return m_jointFeedback;
|
||||
}
|
||||
|
||||
b3JointFeedback* getJointFeedback()
|
||||
{
|
||||
return m_jointFeedback;
|
||||
}
|
||||
|
||||
int getUid() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
bool needsFeedback() const
|
||||
{
|
||||
return m_needsFeedback;
|
||||
}
|
||||
|
||||
///enableFeedback will allow to read the applied linear and angular impulse
|
||||
///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
|
||||
void enableFeedback(bool needsFeedback)
|
||||
{
|
||||
m_needsFeedback = needsFeedback;
|
||||
}
|
||||
|
||||
///getAppliedImpulse is an estimated total applied impulse.
|
||||
///This feedback could be used to determine breaking constraints or playing sounds.
|
||||
b3Scalar getAppliedImpulse() const
|
||||
{
|
||||
b3Assert(m_needsFeedback);
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
b3TypedConstraintType getConstraintType() const
|
||||
{
|
||||
return b3TypedConstraintType(m_objectType);
|
||||
}
|
||||
|
||||
void setDbgDrawSize(b3Scalar dbgDrawSize)
|
||||
{
|
||||
m_dbgDrawSize = dbgDrawSize;
|
||||
}
|
||||
b3Scalar getDbgDrawSize()
|
||||
{
|
||||
return m_dbgDrawSize;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
|
||||
|
||||
///return the local value of parameter
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const = 0;
|
||||
|
||||
// virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
//virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
|
||||
};
|
||||
|
||||
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
|
||||
// all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
|
||||
B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
|
||||
{
|
||||
if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
else if (angleInRadians < angleLowerLimitInRadians)
|
||||
{
|
||||
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
|
||||
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
|
||||
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
|
||||
}
|
||||
else if (angleInRadians > angleUpperLimitInRadians)
|
||||
{
|
||||
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
|
||||
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
|
||||
return (diffLo < diffHi) ? (angleInRadians - B3_2_PI) : angleInRadians;
|
||||
}
|
||||
else
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
}
|
||||
|
||||
// clang-format off
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct b3TypedConstraintData
|
||||
{
|
||||
int m_bodyA;
|
||||
int m_bodyB;
|
||||
char *m_name;
|
||||
|
||||
int m_objectType;
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
int m_needsFeedback;
|
||||
|
||||
float m_appliedImpulse;
|
||||
float m_dbgDrawSize;
|
||||
|
||||
int m_disableCollisionsBetweenLinkedBodies;
|
||||
int m_overrideNumSolverIterations;
|
||||
|
||||
float m_breakingImpulseThreshold;
|
||||
int m_isEnabled;
|
||||
|
||||
};
|
||||
|
||||
// clang-format on
|
||||
|
||||
/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(b3TypedConstraintData);
|
||||
}
|
||||
*/
|
||||
|
||||
class b3AngularLimit
|
||||
{
|
||||
private:
|
||||
b3Scalar
|
||||
m_center,
|
||||
m_halfRange,
|
||||
m_softness,
|
||||
m_biasFactor,
|
||||
m_relaxationFactor,
|
||||
m_correction,
|
||||
m_sign;
|
||||
|
||||
bool
|
||||
m_solveLimit;
|
||||
|
||||
public:
|
||||
/// Default constructor initializes limit as inactive, allowing free constraint movement
|
||||
b3AngularLimit()
|
||||
: m_center(0.0f),
|
||||
m_halfRange(-1.0f),
|
||||
m_softness(0.9f),
|
||||
m_biasFactor(0.3f),
|
||||
m_relaxationFactor(1.0f),
|
||||
m_correction(0.0f),
|
||||
m_sign(0.0f),
|
||||
m_solveLimit(false)
|
||||
{
|
||||
}
|
||||
|
||||
/// Sets all limit's parameters.
|
||||
/// When low > high limit becomes inactive.
|
||||
/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
|
||||
void set(b3Scalar low, b3Scalar high, b3Scalar _softness = 0.9f, b3Scalar _biasFactor = 0.3f, b3Scalar _relaxationFactor = 1.0f);
|
||||
|
||||
/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
|
||||
/// correction is calculated.
|
||||
void test(const b3Scalar angle);
|
||||
|
||||
/// Returns limit's softness
|
||||
inline b3Scalar getSoftness() const
|
||||
{
|
||||
return m_softness;
|
||||
}
|
||||
|
||||
/// Returns limit's bias factor
|
||||
inline b3Scalar getBiasFactor() const
|
||||
{
|
||||
return m_biasFactor;
|
||||
}
|
||||
|
||||
/// Returns limit's relaxation factor
|
||||
inline b3Scalar getRelaxationFactor() const
|
||||
{
|
||||
return m_relaxationFactor;
|
||||
}
|
||||
|
||||
/// Returns correction value evaluated when test() was invoked
|
||||
inline b3Scalar getCorrection() const
|
||||
{
|
||||
return m_correction;
|
||||
}
|
||||
|
||||
/// Returns sign value evaluated when test() was invoked
|
||||
inline b3Scalar getSign() const
|
||||
{
|
||||
return m_sign;
|
||||
}
|
||||
|
||||
/// Gives half of the distance between min and max limit angle
|
||||
inline b3Scalar getHalfRange() const
|
||||
{
|
||||
return m_halfRange;
|
||||
}
|
||||
|
||||
/// Returns true when the last test() invocation recognized limit violation
|
||||
inline bool isLimit() const
|
||||
{
|
||||
return m_solveLimit;
|
||||
}
|
||||
|
||||
/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
|
||||
/// returned is modified so it equals to the limit closest to given angle.
|
||||
void fit(b3Scalar& angle) const;
|
||||
|
||||
/// Returns correction value multiplied by sign value
|
||||
b3Scalar getError() const;
|
||||
|
||||
b3Scalar getLow() const;
|
||||
|
||||
b3Scalar getHigh() const;
|
||||
};
|
||||
|
||||
#endif //B3_TYPED_CONSTRAINT_H
|
||||
|
|
@ -1,447 +0,0 @@
|
|||
#include "b3CpuRigidBodyPipeline.h"
|
||||
|
||||
#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
|
||||
#include "Bullet3Dynamics/shared/b3Inertia.h"
|
||||
|
||||
struct b3CpuRigidBodyPipelineInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
|
||||
b3AlignedObjectArray<b3Inertia> m_inertias;
|
||||
b3AlignedObjectArray<b3Aabb> m_aabbWorldSpace;
|
||||
|
||||
b3DynamicBvhBroadphase* m_bp;
|
||||
b3CpuNarrowPhase* m_np;
|
||||
b3Config m_config;
|
||||
};
|
||||
|
||||
b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
|
||||
{
|
||||
m_data = new b3CpuRigidBodyPipelineInternalData;
|
||||
m_data->m_np = narrowphase;
|
||||
m_data->m_bp = broadphaseDbvt;
|
||||
m_data->m_config = config;
|
||||
}
|
||||
|
||||
b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
|
||||
{
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
|
||||
{
|
||||
for (int i = 0; i < this->getNumBodies(); i++)
|
||||
{
|
||||
b3RigidBodyData* body = &m_data->m_rigidBodies[i];
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
|
||||
int shapeIndex = collidable.m_shapeIndex;
|
||||
|
||||
if (shapeIndex >= 0)
|
||||
{
|
||||
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
|
||||
b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
|
||||
float margin = 0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
|
||||
m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
|
||||
{
|
||||
int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
|
||||
m_data->m_bp->calculateOverlappingPairs();
|
||||
numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
|
||||
printf("numPairs=%d\n", numPairs);
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::computeContactPoints()
|
||||
{
|
||||
b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
|
||||
|
||||
m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
|
||||
}
|
||||
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
{
|
||||
//update world space aabb's
|
||||
updateAabbWorldSpace();
|
||||
|
||||
//compute overlapping pairs
|
||||
computeOverlappingPairs();
|
||||
|
||||
//compute contacts
|
||||
computeContactPoints();
|
||||
|
||||
//solve contacts
|
||||
|
||||
//update transforms
|
||||
integrate(deltaTime);
|
||||
}
|
||||
|
||||
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
|
||||
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
|
||||
{
|
||||
return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
|
||||
}
|
||||
|
||||
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
|
||||
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
|
||||
{
|
||||
linear = -n;
|
||||
angular0 = -b3Cross(r0, n);
|
||||
angular1 = b3Cross(r1, n);
|
||||
}
|
||||
|
||||
static inline void b3SolveContact(b3ContactConstraint4& cs,
|
||||
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
|
||||
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
|
||||
float maxRambdaDt[4], float minRambdaDt[4])
|
||||
{
|
||||
b3Vector3 dLinVelA;
|
||||
dLinVelA.setZero();
|
||||
b3Vector3 dAngVelA;
|
||||
dAngVelA.setZero();
|
||||
b3Vector3 dLinVelB;
|
||||
dLinVelB.setZero();
|
||||
b3Vector3 dAngVelB;
|
||||
dAngVelB.setZero();
|
||||
|
||||
for (int ic = 0; ic < 4; ic++)
|
||||
{
|
||||
// dont necessary because this makes change to 0
|
||||
if (cs.m_jacCoeffInv[ic] == 0.f) continue;
|
||||
|
||||
{
|
||||
b3Vector3 angular0, angular1, linear;
|
||||
b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
|
||||
b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
|
||||
b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
|
||||
|
||||
float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
|
||||
linVelA, angVelA, linVelB, angVelB) +
|
||||
cs.m_b[ic];
|
||||
rambdaDt *= cs.m_jacCoeffInv[ic];
|
||||
|
||||
{
|
||||
float prevSum = cs.m_appliedRambdaDt[ic];
|
||||
float updated = prevSum;
|
||||
updated += rambdaDt;
|
||||
updated = b3Max(updated, minRambdaDt[ic]);
|
||||
updated = b3Min(updated, maxRambdaDt[ic]);
|
||||
rambdaDt = updated - prevSum;
|
||||
cs.m_appliedRambdaDt[ic] = updated;
|
||||
}
|
||||
|
||||
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
|
||||
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
|
||||
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
|
||||
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(linImp0.getX()));
|
||||
b3Assert(_finite(linImp1.getX()));
|
||||
#endif
|
||||
{
|
||||
linVelA += linImp0;
|
||||
angVelA += angImp0;
|
||||
linVelB += linImp1;
|
||||
angVelB += angImp1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static inline void b3SolveFriction(b3ContactConstraint4& cs,
|
||||
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
|
||||
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
|
||||
float maxRambdaDt[4], float minRambdaDt[4])
|
||||
{
|
||||
if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
|
||||
const b3Vector3& center = (const b3Vector3&)cs.m_center;
|
||||
|
||||
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
|
||||
|
||||
b3Vector3 tangent[2];
|
||||
|
||||
b3PlaneSpace1(n, tangent[0], tangent[1]);
|
||||
|
||||
b3Vector3 angular0, angular1, linear;
|
||||
b3Vector3 r0 = center - posA;
|
||||
b3Vector3 r1 = center - posB;
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
|
||||
float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
|
||||
linVelA, angVelA, linVelB, angVelB);
|
||||
rambdaDt *= cs.m_fJacCoeffInv[i];
|
||||
|
||||
{
|
||||
float prevSum = cs.m_fAppliedRambdaDt[i];
|
||||
float updated = prevSum;
|
||||
updated += rambdaDt;
|
||||
updated = b3Max(updated, minRambdaDt[i]);
|
||||
updated = b3Min(updated, maxRambdaDt[i]);
|
||||
rambdaDt = updated - prevSum;
|
||||
cs.m_fAppliedRambdaDt[i] = updated;
|
||||
}
|
||||
|
||||
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
|
||||
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
|
||||
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
|
||||
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(linImp0.getX()));
|
||||
b3Assert(_finite(linImp1.getX()));
|
||||
#endif
|
||||
linVelA += linImp0;
|
||||
angVelA += angImp0;
|
||||
linVelB += linImp1;
|
||||
angVelB += angImp1;
|
||||
}
|
||||
|
||||
{ // angular damping for point constraint
|
||||
b3Vector3 ab = (posB - posA).normalized();
|
||||
b3Vector3 ac = (center - posA).normalized();
|
||||
if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
|
||||
{
|
||||
float angNA = b3Dot(n, angVelA);
|
||||
float angNB = b3Dot(n, angVelB);
|
||||
|
||||
angVelA -= (angNA * 0.1f) * n;
|
||||
angVelB -= (angNB * 0.1f) * n;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct b3SolveTask // : public ThreadPool::Task
|
||||
{
|
||||
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
|
||||
b3AlignedObjectArray<b3Inertia>& shapes,
|
||||
b3AlignedObjectArray<b3ContactConstraint4>& constraints,
|
||||
int start, int nConstraints,
|
||||
int maxNumBatches,
|
||||
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
|
||||
: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
|
||||
{
|
||||
}
|
||||
|
||||
unsigned short int getType() { return 0; }
|
||||
|
||||
void run(int tIdx)
|
||||
{
|
||||
b3AlignedObjectArray<int> usedBodies;
|
||||
//printf("run..............\n");
|
||||
|
||||
for (int bb = 0; bb < m_maxNumBatches; bb++)
|
||||
{
|
||||
usedBodies.resize(0);
|
||||
for (int ic = m_nConstraints - 1; ic >= 0; ic--)
|
||||
//for(int ic=0; ic<m_nConstraints; ic++)
|
||||
{
|
||||
int i = m_start + ic;
|
||||
if (m_constraints[i].m_batchIdx != bb)
|
||||
continue;
|
||||
|
||||
float frictionCoeff = b3GetFrictionCoeff(&m_constraints[i]);
|
||||
int aIdx = (int)m_constraints[i].m_bodyA;
|
||||
int bIdx = (int)m_constraints[i].m_bodyB;
|
||||
//int localBatch = m_constraints[i].m_batchIdx;
|
||||
b3RigidBodyData& bodyA = m_bodies[aIdx];
|
||||
b3RigidBodyData& bodyB = m_bodies[bIdx];
|
||||
|
||||
#if 0
|
||||
if ((bodyA.m_invMass) && (bodyB.m_invMass))
|
||||
{
|
||||
// printf("aIdx=%d, bIdx=%d\n", aIdx,bIdx);
|
||||
}
|
||||
if (bIdx==10)
|
||||
{
|
||||
//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
|
||||
}
|
||||
#endif
|
||||
if (aIdx == 10)
|
||||
{
|
||||
//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
|
||||
}
|
||||
if (usedBodies.size() < (aIdx + 1))
|
||||
{
|
||||
usedBodies.resize(aIdx + 1, 0);
|
||||
}
|
||||
|
||||
if (usedBodies.size() < (bIdx + 1))
|
||||
{
|
||||
usedBodies.resize(bIdx + 1, 0);
|
||||
}
|
||||
|
||||
if (bodyA.m_invMass)
|
||||
{
|
||||
b3Assert(usedBodies[aIdx] == 0);
|
||||
usedBodies[aIdx]++;
|
||||
}
|
||||
|
||||
if (bodyB.m_invMass)
|
||||
{
|
||||
b3Assert(usedBodies[bIdx] == 0);
|
||||
usedBodies[bIdx]++;
|
||||
}
|
||||
|
||||
if (!m_solveFriction)
|
||||
{
|
||||
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
|
||||
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
|
||||
|
||||
b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
|
||||
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
|
||||
maxRambdaDt, minRambdaDt);
|
||||
}
|
||||
else
|
||||
{
|
||||
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
|
||||
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
|
||||
|
||||
float sum = 0;
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
sum += m_constraints[i].m_appliedRambdaDt[j];
|
||||
}
|
||||
frictionCoeff = 0.7f;
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
maxRambdaDt[j] = frictionCoeff * sum;
|
||||
minRambdaDt[j] = -maxRambdaDt[j];
|
||||
}
|
||||
|
||||
b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
|
||||
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
|
||||
maxRambdaDt, minRambdaDt);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_wgUsedBodies)
|
||||
{
|
||||
if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
|
||||
{
|
||||
m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
|
||||
}
|
||||
for (int i = 0; i < usedBodies.size(); i++)
|
||||
{
|
||||
if (usedBodies[i])
|
||||
{
|
||||
//printf("cell %d uses body %d\n", m_curWgidx,i);
|
||||
m_wgUsedBodies[m_curWgidx][i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
|
||||
b3AlignedObjectArray<b3Inertia>& m_shapes;
|
||||
b3AlignedObjectArray<b3ContactConstraint4>& m_constraints;
|
||||
b3AlignedObjectArray<int>* m_wgUsedBodies;
|
||||
int m_curWgidx;
|
||||
int m_start;
|
||||
int m_nConstraints;
|
||||
bool m_solveFriction;
|
||||
int m_maxNumBatches;
|
||||
};
|
||||
|
||||
void b3CpuRigidBodyPipeline::solveContactConstraints()
|
||||
{
|
||||
int m_nIterations = 4;
|
||||
|
||||
b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
|
||||
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
|
||||
int n = contactConstraints.size();
|
||||
//convert contacts...
|
||||
|
||||
int maxNumBatches = 250;
|
||||
|
||||
for (int iter = 0; iter < m_nIterations; iter++)
|
||||
{
|
||||
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
|
||||
task.m_solveFriction = false;
|
||||
task.run(0);
|
||||
}
|
||||
|
||||
for (int iter = 0; iter < m_nIterations; iter++)
|
||||
{
|
||||
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
|
||||
task.m_solveFriction = true;
|
||||
task.run(0);
|
||||
}
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::integrate(float deltaTime)
|
||||
{
|
||||
float angDamping = 0.f;
|
||||
b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
|
||||
|
||||
//integrate transforms (external forces/gravity should be moved into constraint solver)
|
||||
for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
|
||||
{
|
||||
b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
|
||||
}
|
||||
}
|
||||
|
||||
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
|
||||
{
|
||||
b3RigidBodyData body;
|
||||
int bodyIndex = m_data->m_rigidBodies.size();
|
||||
body.m_invMass = mass ? 1.f / mass : 0.f;
|
||||
body.m_angVel.setValue(0, 0, 0);
|
||||
body.m_collidableIdx = collidableIndex;
|
||||
body.m_frictionCoeff = 0.3f;
|
||||
body.m_linVel.setValue(0, 0, 0);
|
||||
body.m_pos.setValue(position[0], position[1], position[2]);
|
||||
body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
|
||||
body.m_restituitionCoeff = 0.f;
|
||||
|
||||
m_data->m_rigidBodies.push_back(body);
|
||||
|
||||
if (collidableIndex >= 0)
|
||||
{
|
||||
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
|
||||
|
||||
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
|
||||
b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
|
||||
b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
|
||||
|
||||
b3Scalar margin = 0.01f;
|
||||
b3Transform t;
|
||||
t.setIdentity();
|
||||
t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
|
||||
t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
|
||||
b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
|
||||
|
||||
m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
|
||||
// b3Vector3 aabbMin,aabbMax;
|
||||
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
|
||||
}
|
||||
|
||||
return bodyIndex;
|
||||
}
|
||||
|
||||
const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
|
||||
{
|
||||
return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
|
||||
}
|
||||
|
||||
int b3CpuRigidBodyPipeline::getNumBodies() const
|
||||
{
|
||||
return m_data->m_rigidBodies.size();
|
||||
}
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2013 Advanced Micro Devices, Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
//Originally written by Erwin Coumans
|
||||
|
||||
#ifndef B3_CPU_RIGIDBODY_PIPELINE_H
|
||||
#define B3_CPU_RIGIDBODY_PIPELINE_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
|
||||
|
||||
class b3CpuRigidBodyPipeline
|
||||
{
|
||||
protected:
|
||||
struct b3CpuRigidBodyPipelineInternalData* m_data;
|
||||
|
||||
int allocateCollidable();
|
||||
|
||||
public:
|
||||
b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
|
||||
virtual ~b3CpuRigidBodyPipeline();
|
||||
|
||||
virtual void stepSimulation(float deltaTime);
|
||||
virtual void integrate(float timeStep);
|
||||
virtual void updateAabbWorldSpace();
|
||||
virtual void computeOverlappingPairs();
|
||||
virtual void computeContactPoints();
|
||||
virtual void solveContactConstraints();
|
||||
|
||||
int registerConvexPolyhedron(class b3ConvexUtility* convex);
|
||||
|
||||
int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
|
||||
void writeAllInstancesToGpu();
|
||||
void copyConstraintsToHost();
|
||||
void setGravity(const float* grav);
|
||||
void reset();
|
||||
|
||||
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold);
|
||||
int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
|
||||
void removeConstraintByUid(int uid);
|
||||
|
||||
void addConstraint(class b3TypedConstraint* constraint);
|
||||
void removeConstraint(b3TypedConstraint* constraint);
|
||||
|
||||
void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
|
||||
|
||||
const struct b3RigidBodyData* getBodyBuffer() const;
|
||||
|
||||
int getNumBodies() const;
|
||||
};
|
||||
|
||||
#endif //B3_CPU_RIGIDBODY_PIPELINE_H
|
||||
|
|
@ -1,18 +0,0 @@
|
|||
project "Bullet3Dynamics"
|
||||
|
||||
language "C++"
|
||||
|
||||
kind "StaticLib"
|
||||
|
||||
includedirs {
|
||||
".."
|
||||
}
|
||||
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
|
||||
files {
|
||||
"**.cpp",
|
||||
"**.h"
|
||||
}
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
#ifndef B3_CONTACT_CONSTRAINT5_H
|
||||
#define B3_CONTACT_CONSTRAINT5_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
|
||||
|
||||
struct b3ContactConstraint4
|
||||
{
|
||||
b3Float4 m_linear; //normal?
|
||||
b3Float4 m_worldPos[4];
|
||||
b3Float4 m_center; // friction
|
||||
float m_jacCoeffInv[4];
|
||||
float m_b[4];
|
||||
float m_appliedRambdaDt[4];
|
||||
float m_fJacCoeffInv[2]; // friction
|
||||
float m_fAppliedRambdaDt[2]; // friction
|
||||
|
||||
unsigned int m_bodyA;
|
||||
unsigned int m_bodyB;
|
||||
int m_batchIdx;
|
||||
unsigned int m_paddings;
|
||||
};
|
||||
|
||||
//inline void setFrictionCoeff(float value) { m_linear[3] = value; }
|
||||
inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
|
||||
{
|
||||
return constraint->m_linear.w;
|
||||
}
|
||||
|
||||
#endif //B3_CONTACT_CONSTRAINT5_H
|
||||
|
|
@ -1,148 +0,0 @@
|
|||
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
|
||||
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
|
||||
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
|
||||
{
|
||||
if (b3Fabs(n.z) > 0.70710678f)
|
||||
{
|
||||
// choose p in y-z plane
|
||||
float a = n.y * n.y + n.z * n.z;
|
||||
float k = 1.f / sqrt(a);
|
||||
p[0].x = 0;
|
||||
p[0].y = -n.z * k;
|
||||
p[0].z = n.y * k;
|
||||
// set q = n x p
|
||||
q[0].x = a * k;
|
||||
q[0].y = -n.x * p[0].z;
|
||||
q[0].z = n.x * p[0].y;
|
||||
}
|
||||
else
|
||||
{
|
||||
// choose p in x-y plane
|
||||
float a = n.x * n.x + n.y * n.y;
|
||||
float k = 1.f / sqrt(a);
|
||||
p[0].x = -n.y * k;
|
||||
p[0].y = n.x * k;
|
||||
p[0].z = 0;
|
||||
// set q = n x p
|
||||
q[0].x = -n.z * p[0].y;
|
||||
q[0].y = n.z * p[0].x;
|
||||
q[0].z = a * k;
|
||||
}
|
||||
}
|
||||
|
||||
void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
|
||||
{
|
||||
*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
|
||||
*angular0 = b3Cross3(r0, n);
|
||||
*angular1 = -b3Cross3(r1, n);
|
||||
}
|
||||
|
||||
float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
|
||||
b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
|
||||
{
|
||||
return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
|
||||
}
|
||||
|
||||
float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
|
||||
float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
|
||||
{
|
||||
// linear0,1 are normlized
|
||||
float jmj0 = invMass0; //b3Dot3F4(linear0, linear0)*invMass0;
|
||||
float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
|
||||
float jmj2 = invMass1; //b3Dot3F4(linear1, linear1)*invMass1;
|
||||
float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
|
||||
return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
|
||||
}
|
||||
|
||||
void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
|
||||
b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
|
||||
__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
|
||||
b3ContactConstraint4_t* dstC)
|
||||
{
|
||||
dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
|
||||
dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
|
||||
|
||||
float dtInv = 1.f / dt;
|
||||
for (int ic = 0; ic < 4; ic++)
|
||||
{
|
||||
dstC->m_appliedRambdaDt[ic] = 0.f;
|
||||
}
|
||||
dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
|
||||
|
||||
dstC->m_linear = src->m_worldNormalOnB;
|
||||
dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() );
|
||||
for (int ic = 0; ic < 4; ic++)
|
||||
{
|
||||
b3Float4 r0 = src->m_worldPosB[ic] - posA;
|
||||
b3Float4 r1 = src->m_worldPosB[ic] - posB;
|
||||
|
||||
if (ic >= src->m_worldNormalOnB.w) //npoints
|
||||
{
|
||||
dstC->m_jacCoeffInv[ic] = 0.f;
|
||||
continue;
|
||||
}
|
||||
|
||||
float relVelN;
|
||||
{
|
||||
b3Float4 linear, angular0, angular1;
|
||||
setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
|
||||
|
||||
dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
|
||||
invMassA, &invInertiaA, invMassB, &invInertiaB);
|
||||
|
||||
relVelN = calcRelVel(linear, -linear, angular0, angular1,
|
||||
linVelA, angVelA, linVelB, angVelB);
|
||||
|
||||
float e = 0.f; //src->getRestituitionCoeff();
|
||||
if (relVelN * relVelN < 0.004f) e = 0.f;
|
||||
|
||||
dstC->m_b[ic] = e * relVelN;
|
||||
//float penetration = src->m_worldPosB[ic].w;
|
||||
dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
|
||||
dstC->m_appliedRambdaDt[ic] = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
if (src->m_worldNormalOnB.w > 0) //npoints
|
||||
{ // prepare friction
|
||||
b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
|
||||
for (int i = 0; i < src->m_worldNormalOnB.w; i++)
|
||||
center += src->m_worldPosB[i];
|
||||
center /= (float)src->m_worldNormalOnB.w;
|
||||
|
||||
b3Float4 tangent[2];
|
||||
b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
|
||||
|
||||
b3Float4 r[2];
|
||||
r[0] = center - posA;
|
||||
r[1] = center - posB;
|
||||
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
b3Float4 linear, angular0, angular1;
|
||||
setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
|
||||
|
||||
dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
|
||||
invMassA, &invInertiaA, invMassB, &invInertiaB);
|
||||
dstC->m_fAppliedRambdaDt[i] = 0.f;
|
||||
}
|
||||
dstC->m_center = center;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
if (i < src->m_worldNormalOnB.w)
|
||||
{
|
||||
dstC->m_worldPos[i] = src->m_worldPosB[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
|
||||
|
||||
#ifndef B3_INERTIA_H
|
||||
#define B3_INERTIA_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Mat3x3.h"
|
||||
|
||||
struct b3Inertia
|
||||
{
|
||||
b3Mat3x3 m_invInertiaWorld;
|
||||
b3Mat3x3 m_initInvInertia;
|
||||
};
|
||||
|
||||
#endif //B3_INERTIA_H
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
|
||||
inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
|
||||
{
|
||||
if (bodies[nodeID].m_invMass != 0.f)
|
||||
{
|
||||
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
|
||||
|
||||
//angular velocity
|
||||
{
|
||||
b3Float4 axis;
|
||||
//add some hardcoded angular damping
|
||||
bodies[nodeID].m_angVel.x *= angularDamping;
|
||||
bodies[nodeID].m_angVel.y *= angularDamping;
|
||||
bodies[nodeID].m_angVel.z *= angularDamping;
|
||||
|
||||
b3Float4 angvel = bodies[nodeID].m_angVel;
|
||||
|
||||
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
|
||||
|
||||
//limit the angular motion
|
||||
if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
|
||||
{
|
||||
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
|
||||
}
|
||||
if (fAngle < 0.001f)
|
||||
{
|
||||
// use Taylor's expansions of sync function
|
||||
axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
|
||||
}
|
||||
else
|
||||
{
|
||||
// sync(fAngle) = sin(c*fAngle)/t
|
||||
axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
|
||||
}
|
||||
|
||||
b3Quat dorn;
|
||||
dorn.x = axis.x;
|
||||
dorn.y = axis.y;
|
||||
dorn.z = axis.z;
|
||||
dorn.w = b3Cos(fAngle * timeStep * 0.5f);
|
||||
b3Quat orn0 = bodies[nodeID].m_quat;
|
||||
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
|
||||
predictedOrn = b3QuatNormalized(predictedOrn);
|
||||
bodies[nodeID].m_quat = predictedOrn;
|
||||
}
|
||||
//linear velocity
|
||||
bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
|
||||
|
||||
//apply gravity
|
||||
bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
|
||||
}
|
||||
}
|
||||
|
||||
inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
|
||||
{
|
||||
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
|
||||
|
||||
if ((body->m_invMass != 0.f))
|
||||
{
|
||||
//angular velocity
|
||||
{
|
||||
b3Float4 axis;
|
||||
//add some hardcoded angular damping
|
||||
body->m_angVel.x *= angularDamping;
|
||||
body->m_angVel.y *= angularDamping;
|
||||
body->m_angVel.z *= angularDamping;
|
||||
|
||||
b3Float4 angvel = body->m_angVel;
|
||||
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
|
||||
//limit the angular motion
|
||||
if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
|
||||
{
|
||||
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
|
||||
}
|
||||
if (fAngle < 0.001f)
|
||||
{
|
||||
// use Taylor's expansions of sync function
|
||||
axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
|
||||
}
|
||||
else
|
||||
{
|
||||
// sync(fAngle) = sin(c*fAngle)/t
|
||||
axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
|
||||
}
|
||||
b3Quat dorn;
|
||||
dorn.x = axis.x;
|
||||
dorn.y = axis.y;
|
||||
dorn.z = axis.z;
|
||||
dorn.w = b3Cos(fAngle * timeStep * 0.5f);
|
||||
b3Quat orn0 = body->m_quat;
|
||||
|
||||
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
|
||||
predictedOrn = b3QuatNormalized(predictedOrn);
|
||||
body->m_quat = predictedOrn;
|
||||
}
|
||||
|
||||
//apply gravity
|
||||
body->m_linVel += gravityAcceleration * timeStep;
|
||||
|
||||
//linear velocity
|
||||
body->m_pos += body->m_linVel * timeStep;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,47 +0,0 @@
|
|||
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src
|
||||
)
|
||||
|
||||
SET(Bullet3Geometry_SRCS
|
||||
b3ConvexHullComputer.cpp
|
||||
b3GeometryUtil.cpp
|
||||
)
|
||||
|
||||
SET(Bullet3Geometry_HDRS
|
||||
b3AabbUtil.h
|
||||
b3ConvexHullComputer.h
|
||||
b3GeometryUtil.h
|
||||
b3GrahamScan2dConvexHull.h
|
||||
)
|
||||
|
||||
ADD_LIBRARY(Bullet3Geometry ${Bullet3Geometry_SRCS} ${Bullet3Geometry_HDRS})
|
||||
if (BUILD_SHARED_LIBS)
|
||||
target_link_libraries(Bullet3Geometry Bullet3Common)
|
||||
endif()
|
||||
SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES SOVERSION ${BULLET_VERSION})
|
||||
|
||||
IF (INSTALL_LIBS)
|
||||
IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
#FILES_MATCHING requires CMake 2.6
|
||||
IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Geometry DESTINATION .)
|
||||
ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
INSTALL(TARGETS Bullet3Geometry
|
||||
RUNTIME DESTINATION bin
|
||||
LIBRARY DESTINATION lib${LIB_SUFFIX}
|
||||
ARCHIVE DESTINATION lib${LIB_SUFFIX})
|
||||
INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN
|
||||
".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
|
||||
|
||||
IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES FRAMEWORK true)
|
||||
SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES PUBLIC_HEADER "${Bullet3Geometry_HDRS}")
|
||||
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
|
||||
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
|
||||
ENDIF (INSTALL_LIBS)
|
||||
|
|
@ -1,217 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_AABB_UTIL2
|
||||
#define B3_AABB_UTIL2
|
||||
|
||||
#include "Bullet3Common/b3Transform.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Common/b3MinMax.h"
|
||||
|
||||
B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
|
||||
b3Vector3& aabbMax,
|
||||
const b3Vector3& expansionMin,
|
||||
const b3Vector3& expansionMax)
|
||||
{
|
||||
aabbMin = aabbMin + expansionMin;
|
||||
aabbMax = aabbMax + expansionMax;
|
||||
}
|
||||
|
||||
/// conservative test for overlap between two aabbs
|
||||
B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
|
||||
const b3Vector3& point)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
|
||||
overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
|
||||
overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
|
||||
return overlap;
|
||||
}
|
||||
|
||||
/// conservative test for overlap between two aabbs
|
||||
B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
|
||||
const b3Vector3& aabbMin2, const b3Vector3& aabbMax2)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
|
||||
overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
|
||||
overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
|
||||
return overlap;
|
||||
}
|
||||
|
||||
/// conservative test for overlap between triangle and aabb
|
||||
B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3* vertices,
|
||||
const b3Vector3& aabbMin, const b3Vector3& aabbMax)
|
||||
{
|
||||
const b3Vector3& p1 = vertices[0];
|
||||
const b3Vector3& p2 = vertices[1];
|
||||
const b3Vector3& p3 = vertices[2];
|
||||
|
||||
if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
|
||||
if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
|
||||
|
||||
if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
|
||||
if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
|
||||
|
||||
if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
|
||||
if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE int b3Outcode(const b3Vector3& p, const b3Vector3& halfExtent)
|
||||
{
|
||||
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
|
||||
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
|
||||
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
|
||||
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
|
||||
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
|
||||
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
|
||||
const b3Vector3& rayInvDirection,
|
||||
const unsigned int raySign[3],
|
||||
const b3Vector3 bounds[2],
|
||||
b3Scalar& tmin,
|
||||
b3Scalar lambda_min,
|
||||
b3Scalar lambda_max)
|
||||
{
|
||||
b3Scalar tmax, tymin, tymax, tzmin, tzmax;
|
||||
tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
|
||||
tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
|
||||
tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
|
||||
tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
|
||||
|
||||
if ((tmin > tymax) || (tymin > tmax))
|
||||
return false;
|
||||
|
||||
if (tymin > tmin)
|
||||
tmin = tymin;
|
||||
|
||||
if (tymax < tmax)
|
||||
tmax = tymax;
|
||||
|
||||
tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
|
||||
tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
|
||||
|
||||
if ((tmin > tzmax) || (tzmin > tmax))
|
||||
return false;
|
||||
if (tzmin > tmin)
|
||||
tmin = tzmin;
|
||||
if (tzmax < tmax)
|
||||
tmax = tzmax;
|
||||
return ((tmin < lambda_max) && (tmax > lambda_min));
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
|
||||
const b3Vector3& rayTo,
|
||||
const b3Vector3& aabbMin,
|
||||
const b3Vector3& aabbMax,
|
||||
b3Scalar& param, b3Vector3& normal)
|
||||
{
|
||||
b3Vector3 aabbHalfExtent = (aabbMax - aabbMin) * b3Scalar(0.5);
|
||||
b3Vector3 aabbCenter = (aabbMax + aabbMin) * b3Scalar(0.5);
|
||||
b3Vector3 source = rayFrom - aabbCenter;
|
||||
b3Vector3 target = rayTo - aabbCenter;
|
||||
int sourceOutcode = b3Outcode(source, aabbHalfExtent);
|
||||
int targetOutcode = b3Outcode(target, aabbHalfExtent);
|
||||
if ((sourceOutcode & targetOutcode) == 0x0)
|
||||
{
|
||||
b3Scalar lambda_enter = b3Scalar(0.0);
|
||||
b3Scalar lambda_exit = param;
|
||||
b3Vector3 r = target - source;
|
||||
int i;
|
||||
b3Scalar normSign = 1;
|
||||
b3Vector3 hitNormal = b3MakeVector3(0, 0, 0);
|
||||
int bit = 1;
|
||||
|
||||
for (int j = 0; j < 2; j++)
|
||||
{
|
||||
for (i = 0; i != 3; ++i)
|
||||
{
|
||||
if (sourceOutcode & bit)
|
||||
{
|
||||
b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
|
||||
if (lambda_enter <= lambda)
|
||||
{
|
||||
lambda_enter = lambda;
|
||||
hitNormal.setValue(0, 0, 0);
|
||||
hitNormal[i] = normSign;
|
||||
}
|
||||
}
|
||||
else if (targetOutcode & bit)
|
||||
{
|
||||
b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
|
||||
b3SetMin(lambda_exit, lambda);
|
||||
}
|
||||
bit <<= 1;
|
||||
}
|
||||
normSign = b3Scalar(-1.);
|
||||
}
|
||||
if (lambda_enter <= lambda_exit)
|
||||
{
|
||||
param = lambda_enter;
|
||||
normal = hitNormal;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
|
||||
{
|
||||
b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(margin, margin, margin);
|
||||
b3Matrix3x3 abs_b = t.getBasis().absolute();
|
||||
b3Vector3 center = t.getOrigin();
|
||||
b3Vector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
|
||||
aabbMinOut = center - extent;
|
||||
aabbMaxOut = center + extent;
|
||||
}
|
||||
|
||||
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
|
||||
{
|
||||
//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
|
||||
//b3Assert(localAabbMin.getY() <= localAabbMax.getY());
|
||||
//b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
|
||||
b3Vector3 localHalfExtents = b3Scalar(0.5) * (localAabbMax - localAabbMin);
|
||||
localHalfExtents += b3MakeVector3(margin, margin, margin);
|
||||
|
||||
b3Vector3 localCenter = b3Scalar(0.5) * (localAabbMax + localAabbMin);
|
||||
b3Matrix3x3 abs_b = trans.getBasis().absolute();
|
||||
b3Vector3 center = trans(localCenter);
|
||||
b3Vector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
|
||||
aabbMinOut = center - extent;
|
||||
aabbMaxOut = center + extent;
|
||||
}
|
||||
|
||||
#define B3_USE_BANCHLESS 1
|
||||
#ifdef B3_USE_BANCHLESS
|
||||
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
|
||||
B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
|
||||
{
|
||||
return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
|
||||
1, 0));
|
||||
}
|
||||
#else
|
||||
B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
|
||||
{
|
||||
bool overlap = true;
|
||||
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
|
||||
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
|
||||
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
|
||||
return overlap;
|
||||
}
|
||||
#endif //B3_USE_BANCHLESS
|
||||
|
||||
#endif //B3_AABB_UTIL2
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,99 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef B3_CONVEX_HULL_COMPUTER_H
|
||||
#define B3_CONVEX_HULL_COMPUTER_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
|
||||
/// Convex hull implementation based on Preparata and Hong
|
||||
/// See http://code.google.com/p/bullet/issues/detail?id=275
|
||||
/// Ole Kniemeyer, MAXON Computer GmbH
|
||||
class b3ConvexHullComputer
|
||||
{
|
||||
private:
|
||||
b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
|
||||
|
||||
public:
|
||||
class Edge
|
||||
{
|
||||
private:
|
||||
int next;
|
||||
int reverse;
|
||||
int targetVertex;
|
||||
|
||||
friend class b3ConvexHullComputer;
|
||||
|
||||
public:
|
||||
int getSourceVertex() const
|
||||
{
|
||||
return (this + reverse)->targetVertex;
|
||||
}
|
||||
|
||||
int getTargetVertex() const
|
||||
{
|
||||
return targetVertex;
|
||||
}
|
||||
|
||||
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
|
||||
{
|
||||
return this + next;
|
||||
}
|
||||
|
||||
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
|
||||
{
|
||||
return (this + reverse)->getNextEdgeOfVertex();
|
||||
}
|
||||
|
||||
const Edge* getReverseEdge() const
|
||||
{
|
||||
return this + reverse;
|
||||
}
|
||||
};
|
||||
|
||||
// Vertices of the output hull
|
||||
b3AlignedObjectArray<b3Vector3> vertices;
|
||||
|
||||
// Edges of the output hull
|
||||
b3AlignedObjectArray<Edge> edges;
|
||||
|
||||
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
|
||||
b3AlignedObjectArray<int> faces;
|
||||
|
||||
/*
|
||||
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
|
||||
between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
|
||||
by that amount (each face is moved by "shrink" length units towards the center along its normal).
|
||||
If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
|
||||
is the minimum distance of a face to the center of the convex hull.
|
||||
|
||||
The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
|
||||
that the resulting convex hull is empty.
|
||||
|
||||
The output convex hull can be found in the member variables "vertices", "edges", "faces".
|
||||
*/
|
||||
b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
|
||||
{
|
||||
return compute(coords, false, stride, count, shrink, shrinkClamp);
|
||||
}
|
||||
|
||||
// same as above, but double precision
|
||||
b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
|
||||
{
|
||||
return compute(coords, true, stride, count, shrink, shrinkClamp);
|
||||
}
|
||||
};
|
||||
|
||||
#endif //B3_CONVEX_HULL_COMPUTER_H
|
||||
|
|
@ -1,174 +0,0 @@
|
|||
/*
|
||||
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "b3GeometryUtil.h"
|
||||
|
||||
/*
|
||||
Make sure this dummy function never changes so that it
|
||||
can be used by probes that are checking whether the
|
||||
library is actually installed.
|
||||
*/
|
||||
extern "C"
|
||||
{
|
||||
void b3BulletMathProbe();
|
||||
|
||||
void b3BulletMathProbe() {}
|
||||
}
|
||||
|
||||
bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
|
||||
{
|
||||
int numbrushes = planeEquations.size();
|
||||
for (int i = 0; i < numbrushes; i++)
|
||||
{
|
||||
const b3Vector3& N1 = planeEquations[i];
|
||||
b3Scalar dist = b3Scalar(N1.dot(point)) + b3Scalar(N1[3]) - margin;
|
||||
if (dist > b3Scalar(0.))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
|
||||
{
|
||||
int numvertices = vertices.size();
|
||||
for (int i = 0; i < numvertices; i++)
|
||||
{
|
||||
const b3Vector3& N1 = vertices[i];
|
||||
b3Scalar dist = b3Scalar(planeNormal.dot(N1)) + b3Scalar(planeNormal[3]) - margin;
|
||||
if (dist > b3Scalar(0.))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations);
|
||||
|
||||
bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations)
|
||||
{
|
||||
int numbrushes = planeEquations.size();
|
||||
for (int i = 0; i < numbrushes; i++)
|
||||
{
|
||||
const b3Vector3& N1 = planeEquations[i];
|
||||
if (planeEquation.dot(N1) > b3Scalar(0.999))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut)
|
||||
{
|
||||
const int numvertices = vertices.size();
|
||||
// brute force:
|
||||
for (int i = 0; i < numvertices; i++)
|
||||
{
|
||||
const b3Vector3& N1 = vertices[i];
|
||||
|
||||
for (int j = i + 1; j < numvertices; j++)
|
||||
{
|
||||
const b3Vector3& N2 = vertices[j];
|
||||
|
||||
for (int k = j + 1; k < numvertices; k++)
|
||||
{
|
||||
const b3Vector3& N3 = vertices[k];
|
||||
|
||||
b3Vector3 planeEquation, edge0, edge1;
|
||||
edge0 = N2 - N1;
|
||||
edge1 = N3 - N1;
|
||||
b3Scalar normalSign = b3Scalar(1.);
|
||||
for (int ww = 0; ww < 2; ww++)
|
||||
{
|
||||
planeEquation = normalSign * edge0.cross(edge1);
|
||||
if (planeEquation.length2() > b3Scalar(0.0001))
|
||||
{
|
||||
planeEquation.normalize();
|
||||
if (notExist(planeEquation, planeEquationsOut))
|
||||
{
|
||||
planeEquation[3] = -planeEquation.dot(N1);
|
||||
|
||||
//check if inside, and replace supportingVertexOut if needed
|
||||
if (areVerticesBehindPlane(planeEquation, vertices, b3Scalar(0.01)))
|
||||
{
|
||||
planeEquationsOut.push_back(planeEquation);
|
||||
}
|
||||
}
|
||||
}
|
||||
normalSign = b3Scalar(-1.);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut)
|
||||
{
|
||||
const int numbrushes = planeEquations.size();
|
||||
// brute force:
|
||||
for (int i = 0; i < numbrushes; i++)
|
||||
{
|
||||
const b3Vector3& N1 = planeEquations[i];
|
||||
|
||||
for (int j = i + 1; j < numbrushes; j++)
|
||||
{
|
||||
const b3Vector3& N2 = planeEquations[j];
|
||||
|
||||
for (int k = j + 1; k < numbrushes; k++)
|
||||
{
|
||||
const b3Vector3& N3 = planeEquations[k];
|
||||
|
||||
b3Vector3 n2n3;
|
||||
n2n3 = N2.cross(N3);
|
||||
b3Vector3 n3n1;
|
||||
n3n1 = N3.cross(N1);
|
||||
b3Vector3 n1n2;
|
||||
n1n2 = N1.cross(N2);
|
||||
|
||||
if ((n2n3.length2() > b3Scalar(0.0001)) &&
|
||||
(n3n1.length2() > b3Scalar(0.0001)) &&
|
||||
(n1n2.length2() > b3Scalar(0.0001)))
|
||||
{
|
||||
//point P out of 3 plane equations:
|
||||
|
||||
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
|
||||
//P = -------------------------------------------------------------------------
|
||||
// N1 . ( N2 * N3 )
|
||||
|
||||
b3Scalar quotient = (N1.dot(n2n3));
|
||||
if (b3Fabs(quotient) > b3Scalar(0.000001))
|
||||
{
|
||||
quotient = b3Scalar(-1.) / quotient;
|
||||
n2n3 *= N1[3];
|
||||
n3n1 *= N2[3];
|
||||
n1n2 *= N3[3];
|
||||
b3Vector3 potentialVertex = n2n3;
|
||||
potentialVertex += n3n1;
|
||||
potentialVertex += n1n2;
|
||||
potentialVertex *= quotient;
|
||||
|
||||
//check if inside, and replace supportingVertexOut if needed
|
||||
if (isPointInsidePlanes(planeEquations, potentialVertex, b3Scalar(0.01)))
|
||||
{
|
||||
verticesOut.push_back(potentialVertex);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue