291 lines
9.9 KiB
C++
291 lines
9.9 KiB
C++
#include <iostream>
|
|
#include <OgreBullet.h>
|
|
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
|
|
#include <btBulletCollisionCommon.h>
|
|
#include <btBulletDynamicsCommon.h>
|
|
|
|
struct objects {
|
|
btDynamicsWorld *world;
|
|
btRigidBody *groundBody;
|
|
btPairCachingGhostObject *characterBody;
|
|
btPairCachingGhostObject *waterBody;
|
|
};
|
|
void setupGround(btDynamicsWorld *world, struct objects *objects)
|
|
{
|
|
// Static ground
|
|
btCollisionShape *groundShape =
|
|
new btBoxShape(btVector3(1000, 1, 1000));
|
|
btDefaultMotionState *groundMotionState = new btDefaultMotionState(
|
|
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -10, 0)));
|
|
btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(
|
|
0, groundMotionState, groundShape, btVector3(0, 0, 0));
|
|
btRigidBody *groundRigidBody = new btRigidBody(groundRigidBodyCI);
|
|
world->addRigidBody(groundRigidBody, 2, 0x7fffffff & ~16);
|
|
objects->groundBody = groundRigidBody;
|
|
}
|
|
void setupCharacter(btDynamicsWorld *world, struct objects *objects)
|
|
{
|
|
btCompoundShape *shape = new btCompoundShape();
|
|
btCapsuleShape *subshape = new btCapsuleShape(0.2f, 1.5f);
|
|
btTransform shapePos;
|
|
shapePos.setIdentity();
|
|
shapePos.setOrigin(btVector3(0, 0.75f, 0));
|
|
shape->addChildShape(shapePos, subshape);
|
|
btPairCachingGhostObject *characterBody =
|
|
new btPairCachingGhostObject();
|
|
characterBody->setCollisionFlags(
|
|
characterBody->getCollisionFlags() |
|
|
btCollisionObject::CF_NO_CONTACT_RESPONSE |
|
|
btCollisionObject::CF_KINEMATIC_OBJECT);
|
|
btTransform bodyPos;
|
|
bodyPos.setIdentity();
|
|
bodyPos.setOrigin(btVector3(0, 0.4f, 0));
|
|
characterBody->setWorldTransform(bodyPos);
|
|
characterBody->setCollisionShape(shape);
|
|
world->addCollisionObject(characterBody, 1, 0x7fffffff);
|
|
objects->characterBody = characterBody;
|
|
}
|
|
void setupWater(btDynamicsWorld *world, struct objects *objects)
|
|
{
|
|
btCompoundShape *shape = new btCompoundShape();
|
|
btBoxShape *subshape =
|
|
new btBoxShape(btVector3(1000.0f, 100.0f, 1000.0f));
|
|
btTransform shapePos;
|
|
shapePos.setIdentity();
|
|
shapePos.setOrigin(btVector3(0, -100, 0));
|
|
shape->addChildShape(shapePos, subshape);
|
|
btPairCachingGhostObject *waterBody = new btPairCachingGhostObject();
|
|
waterBody->setCollisionFlags(waterBody->getCollisionFlags() |
|
|
btCollisionObject::CF_NO_CONTACT_RESPONSE |
|
|
btCollisionObject::CF_KINEMATIC_OBJECT);
|
|
btTransform bodyPos;
|
|
bodyPos.setIdentity();
|
|
waterBody->setWorldTransform(bodyPos);
|
|
waterBody->setCollisionShape(shape);
|
|
world->addCollisionObject(waterBody, 16, 0x7fffffff & ~2);
|
|
objects->waterBody = waterBody;
|
|
}
|
|
struct DeepPenetrationContactResultCallback : public btManifoldResult {
|
|
DeepPenetrationContactResultCallback(
|
|
const btCollisionObjectWrapper *body0Wrap,
|
|
const btCollisionObjectWrapper *body1Wrap)
|
|
: btManifoldResult(body0Wrap, body1Wrap)
|
|
, mPenetrationDistance(0)
|
|
, mOtherIndex(0)
|
|
{
|
|
}
|
|
float mPenetrationDistance;
|
|
int mOtherIndex;
|
|
btVector3 mNormal, mPoint;
|
|
void reset()
|
|
{
|
|
mPenetrationDistance = 0.0f;
|
|
}
|
|
bool hasHit()
|
|
{
|
|
return mPenetrationDistance < 0.0f;
|
|
}
|
|
virtual void addContactPoint(const btVector3 &normalOnBInWorld,
|
|
const btVector3 &pointInWorldOnB,
|
|
btScalar depth)
|
|
{
|
|
std::cout
|
|
<< "contact: " << Ogre::Bullet::convert(pointInWorldOnB)
|
|
<< " " << Ogre::Bullet::convert(normalOnBInWorld)
|
|
<< "\n";
|
|
if (mPenetrationDistance > depth) { // Has penetration?
|
|
|
|
const bool isSwapped =
|
|
m_manifoldPtr->getBody0() !=
|
|
m_body0Wrap->getCollisionObject();
|
|
mPenetrationDistance = depth;
|
|
mOtherIndex = isSwapped ? m_index0 : m_index1;
|
|
mPoint = isSwapped ? (pointInWorldOnB +
|
|
(normalOnBInWorld * depth)) :
|
|
pointInWorldOnB;
|
|
|
|
mNormal = isSwapped ? normalOnBInWorld * -1 :
|
|
normalOnBInWorld;
|
|
}
|
|
}
|
|
};
|
|
void dumpCompoundShape(const char *what, const btCollisionObject *body)
|
|
{
|
|
if (body->getCollisionShape()->isCompound()) {
|
|
const btCompoundShape *shape =
|
|
static_cast<const btCompoundShape *>(
|
|
body->getCollisionShape());
|
|
int i;
|
|
for (i = 0; i < shape->getNumChildShapes(); i++) {
|
|
btTransform transform = body->getWorldTransform() *
|
|
shape->getChildTransform(i);
|
|
std::cout << what << ": " << " shape: " << i << ": "
|
|
<< transform.getOrigin().getX() << ", ";
|
|
std::cout << transform.getOrigin().getY() << ", ";
|
|
std::cout << transform.getOrigin().getZ();
|
|
std::cout << " convex: "
|
|
<< shape->getChildShape(i)->isConvex();
|
|
std::cout << " shape name: "
|
|
<< shape->getChildShape(i)->getName();
|
|
switch (shape->getChildShape(i)->getShapeType()) {
|
|
case 0: {
|
|
const btBoxShape *box =
|
|
static_cast<const btBoxShape *>(
|
|
shape->getChildShape(i));
|
|
btVector3 hextents =
|
|
box->getHalfExtentsWithoutMargin();
|
|
std::cout << " box: " << hextents.getX() << ", "
|
|
<< hextents.getY() << ", "
|
|
<< hextents.getZ();
|
|
} break;
|
|
case 10: {
|
|
const btCapsuleShape *capsule =
|
|
static_cast<const btCapsuleShape *>(
|
|
shape->getChildShape(i));
|
|
float hh = capsule->getHalfHeight();
|
|
float r = capsule->getRadius();
|
|
std::cout << " capsule: " << hh << ", " << r;
|
|
} break;
|
|
default:
|
|
std::cout << " shape type: "
|
|
<< shape->getChildShape(i)
|
|
->getShapeType();
|
|
break;
|
|
}
|
|
std::cout << std::endl;
|
|
}
|
|
}
|
|
}
|
|
void showContacts(btCollisionWorld *world, struct objects *objects)
|
|
{
|
|
int numManifolds = world->getDispatcher()->getNumManifolds();
|
|
if (numManifolds == 0)
|
|
std::cout << "No contacts in world\n";
|
|
for (int i = 0; i < numManifolds; i++) {
|
|
btPersistentManifold *contactManifold =
|
|
world->getDispatcher()->getManifoldByIndexInternal(i);
|
|
const btCollisionObject *obA = contactManifold->getBody0();
|
|
const btCollisionObject *obB = contactManifold->getBody1();
|
|
|
|
int numContacts = contactManifold->getNumContacts();
|
|
for (int j = 0; j < numContacts; j++) {
|
|
btManifoldPoint &pt =
|
|
contactManifold->getContactPoint(j);
|
|
if (pt.getDistance() < 0.f) {
|
|
const btVector3 &ptA = pt.getPositionWorldOnA();
|
|
const btVector3 &ptB = pt.getPositionWorldOnB();
|
|
const btVector3 &normalOnB =
|
|
pt.m_normalWorldOnB;
|
|
std::cout << "contact: " << i << " " << j << " "
|
|
<< ptA.getX() << ", " << ptA.getY()
|
|
<< ", " << ptA.getZ() << std::endl;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
int main()
|
|
{
|
|
struct objects objects;
|
|
btDefaultCollisionConfiguration collisionConfig;
|
|
btCollisionDispatcher dispatcher(&collisionConfig);
|
|
btCollisionDispatcher *dispatch = &dispatcher;
|
|
btDbvtBroadphase broadphase;
|
|
btSequentialImpulseConstraintSolver solver;
|
|
btDiscreteDynamicsWorld world(&dispatcher, &broadphase, &solver,
|
|
&collisionConfig);
|
|
world.setGravity(btVector3(0, -9.81, 0));
|
|
setupGround(&world, &objects);
|
|
setupCharacter(&world, &objects);
|
|
setupWater(&world, &objects);
|
|
|
|
#if 0
|
|
btCompoundShape *compoundShape = new btCompoundShape();
|
|
|
|
// Add shapes to the compound shape
|
|
for (int i = 0; i < 2; ++i) {
|
|
btBoxShape *box = new btBoxShape(btVector3(1, 1, 1));
|
|
btTransform transform;
|
|
transform.setIdentity();
|
|
transform.setOrigin(btVector3(0, 1 - i, 0));
|
|
compoundShape->addChildShape(transform, box);
|
|
}
|
|
|
|
btDefaultMotionState *compoundMotionState = new btDefaultMotionState(
|
|
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 10, 0)));
|
|
btScalar mass = 5.0;
|
|
btVector3 inertia(0, 0, 0);
|
|
compoundShape->calculateLocalInertia(mass, inertia);
|
|
btRigidBody::btRigidBodyConstructionInfo compoundRigidBodyCI(
|
|
mass, compoundMotionState, compoundShape, inertia);
|
|
btRigidBody *compoundRigidBody = new btRigidBody(compoundRigidBodyCI);
|
|
world.addRigidBody(compoundRigidBody);
|
|
#endif
|
|
|
|
// Simulation
|
|
btVector3 velocity(0, 0, 0);
|
|
for (int i = 0; i < 300; i++) {
|
|
world.stepSimulation(1.f / 60.f, 10);
|
|
showContacts(&world, &objects);
|
|
|
|
#if 0
|
|
btTransform transform;
|
|
compoundRigidBody->getMotionState()->getWorldTransform(
|
|
transform);
|
|
std::cout << "Step " << i
|
|
<< ": Position = " << transform.getOrigin().getX()
|
|
<< ", " << transform.getOrigin().getY() << ", "
|
|
<< transform.getOrigin().getZ() << std::endl;
|
|
std::cout << "water overlaps:"
|
|
<< objects.waterBody->getOverlappingPairCache()
|
|
->getNumOverlappingPairs()
|
|
<< "\n";
|
|
#endif
|
|
btTransform transform;
|
|
transform = objects.characterBody->getWorldTransform();
|
|
std::cout << "Step " << i << " ";
|
|
std::cout << "Character: "
|
|
<< "Position = " << transform.getOrigin().getX()
|
|
<< ", " << transform.getOrigin().getY() << ", "
|
|
<< transform.getOrigin().getZ() << std::endl;
|
|
transform = objects.waterBody->getWorldTransform();
|
|
std::cout << "Water: "
|
|
<< "Position = " << transform.getOrigin().getX()
|
|
<< ", " << transform.getOrigin().getY() << ", "
|
|
<< transform.getOrigin().getZ() << std::endl;
|
|
std::cout << "water overlaps:"
|
|
<< objects.waterBody->getOverlappingPairCache()
|
|
->getNumOverlappingPairs()
|
|
<< "\n";
|
|
dumpCompoundShape("Character", objects.characterBody);
|
|
dumpCompoundShape("Water", objects.waterBody);
|
|
btCollisionObjectWrapper obA(
|
|
NULL, objects.waterBody->getCollisionShape(),
|
|
objects.waterBody,
|
|
objects.waterBody->getWorldTransform(), -1, 0);
|
|
btCollisionObjectWrapper obB(
|
|
NULL, objects.characterBody->getCollisionShape(),
|
|
objects.characterBody,
|
|
objects.characterBody->getWorldTransform(), -1, 0);
|
|
std::cout << __func__ << ": call findAlgorithm: ";
|
|
btCollisionAlgorithm *algorithm = dispatch->findAlgorithm(
|
|
&obA, &obB, NULL, BT_CONTACT_POINT_ALGORITHMS);
|
|
std::cout << "found algorithm: " << algorithm << "\n";
|
|
if (!algorithm)
|
|
continue;
|
|
std::cout << "algorithm: " << algorithm << "\n";
|
|
DeepPenetrationContactResultCallback contactPointResult(&obA,
|
|
&obB);
|
|
std::cout << "process collision\n";
|
|
algorithm->processCollision(&obA, &obB, world.getDispatchInfo(),
|
|
&contactPointResult);
|
|
algorithm->~btCollisionAlgorithm();
|
|
dispatch->freeCollisionAlgorithm(algorithm);
|
|
if (contactPointResult.hasHit()) {
|
|
std::cout << "InWater!!1\n";
|
|
}
|
|
velocity += btVector3(0, -9.8, 0) * 1.0f / 16.0f;
|
|
objects.characterBody->getWorldTransform().getOrigin() += velocity * 1.0f / 16.0f;
|
|
std::cout << "process collision done\n";
|
|
}
|
|
}
|