AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans 2016-08-02 08:11:27 -07:00
commit 2597e9eddb
15 changed files with 554 additions and 246 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@ -157,6 +157,7 @@ void deleteDemo()
}
const char* gPngFileName = 0;
int gPngSkipFrames = 0;
@ -763,7 +764,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
loadCurrentSettings(startFileName, args);
args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep);
args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames);
///The OpenCL rigid body pipeline is experimental and
///most OpenCL drivers and OpenCL compilers have issues with our kernels.
///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers
@ -1101,24 +1102,6 @@ void OpenGLExampleBrowser::update(float deltaTime)
//printf("---------------------------------------------------\n");
//printf("Framecount = %d\n",frameCount);
if (gPngFileName)
{
static int skip = 0;
skip++;
if (skip>4)
{
skip=0;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);
s_app->dumpNextFrameToPng(staticPngFileName);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
}
if (gFixedTimeStep>0)
{
@ -1153,6 +1136,25 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
}
if (gPngFileName)
{
static int skip = 0;
skip--;
if (skip<0)
{
skip=gPngSkipFrames;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);
s_app->dumpNextFrameToPng(staticPngFileName);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
}
{
if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())

76
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@ -28,6 +28,7 @@ subject to the following restrictions:
#include "btMatrix4x4.h"
#define MAX_VISUAL_SHAPES 512
struct VertexSource
@ -288,42 +289,47 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
}//for each mesh
int shapeIndex = visualShapes.size();
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
if (shapeIndex<MAX_VISUAL_SHAPES)
{
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
btAssert(vertexNormals.size()==vertexPositions.size());
for (int v=0;v<vertexPositions.size();v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
btAssert(vertexNormals.size()==vertexPositions.size());
for (int v=0;v<vertexPositions.size();v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
for (int index=0;index<indices.size();index++)
{
visualShape.m_indices->push_back(indices[index]+indexBase);
}
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase=visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName,shapeIndex);
for (int index=0;index<indices.size();index++)
{
visualShape.m_indices->push_back(indices[index]+indexBase);
}
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase=visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName,shapeIndex);
} else
{
b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES);
}
}//for each geometry
}
@ -557,7 +563,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
// GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(32);
visualShapes.reserve(MAX_VISUAL_SHAPES);
float extraScaling = 1;//0.01;
btHashMap<btHashString, int> name2ShapeIndex;

2
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@ -67,7 +67,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
} else
{
b3Warning("not found %s\n",relativeFileName);
b3Warning("not found [%s]\n",relativeFileName);
}
}
}

2
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@ -84,7 +84,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian)
// Update target positions
void UpdateTargets( double T2, Tree & treeY) {
double T = T2 / 20.;
double T = T2 / 5.;
target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
}

20
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@ -750,11 +750,13 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
void SimpleOpenGL3App::swapBuffer()
{
m_window->endRendering();
if (m_data->m_frameDumpPngFileName)
{
writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
(int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight();
writeTextureToFile(width,
height,m_data->m_frameDumpPngFileName,
m_data->m_ffmpegFile);
m_data->m_renderTexture->disable();
if (m_data->m_ffmpegFile==0)
@ -762,7 +764,8 @@ void SimpleOpenGL3App::swapBuffer()
m_data->m_frameDumpPngFileName = 0;
}
}
m_window->startRendering();
m_window->endRendering();
m_window->startRendering();
}
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
@ -773,12 +776,15 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
char cmd[8192];
#ifdef _WIN32
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
//sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
#else
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName);
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "

3
examples/OpenGLWindow/X11OpenGLWindow.cpp
View File

@ -516,6 +516,9 @@ void X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_data->m_dpy = MyXOpenDisplay(NULL);
m_data->m_glWidth = ci.m_width;
m_data->m_glHeight = ci.m_height;
if(m_data->m_dpy == NULL) {
printf("\n\tcannot connect to X server\n\n");
exit(0);

7
examples/ThirdPartyLibs/BussIK/LinearR4.h
View File

@ -580,8 +580,11 @@ inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A)
inline void Matrix4x4::SetIdentity ( )
{
m11 = m22 = m33 = m44 = 1.0;
m12 = m13 = m14 = m21 = m23 = m24 = m13 = m23 = m41= m42 = m43 = 0.0;
m12 = m13 = m14 =
m21 = m23 = m24 =
m31 = m32 = m34 =
m41 = m42 = m43 = 0.0;
m11 = m22 = m33 = m44 = 1.0;
}
inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v,

2
examples/ThirdPartyLibs/BussIK/MatrixRmn.h
View File

@ -399,4 +399,4 @@ inline void MatrixRmn::AddArrayScale( long length, const double* from, long from
#endif MATRIX_RMN_H
#endif //MATRIX_RMN_H

2
examples/ThirdPartyLibs/BussIK/VectorRn.h
View File

@ -235,4 +235,4 @@ inline double Dot( const VectorRn& u, const VectorRn& v )
return res;
}
#endif VECTOR_RN_H
#endif //VECTOR_RN_H

2
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@ -366,6 +366,8 @@ std::string LoadMtl (
continue;
}
linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
// Skip leading space.
const char* token = linebuf.c_str();
token += strspn(token, " \t");

79
examples/pybullet/pybullet.c
View File

@ -294,27 +294,27 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
//todo(erwincoumans): set max forces, kp, kd
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double targetPosition=0;
double targetVelocity=0;
double maxForce=100000;
double gains=0.1;
double kp=0.1;
double kd=1.0;
int valid = 0;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
size= PySequence_Size(args);
if (size==4)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@ -324,7 +324,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==5)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@ -334,18 +335,38 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==6)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==8)
{
// only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD)
{
PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD");
return NULL;
}
if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8)
{
PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity");
return NULL;
}
if (valid)
{
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@ -357,7 +378,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
@ -365,19 +386,18 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
PyErr_SetString(SpamError, "Illegral control mode.");
return NULL;
}
commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
double kd = gains;
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetKd(commandHandle,info.m_uIndex,kd);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
@ -386,31 +406,30 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
double kp = gains;
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
default:
{
}
};
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "error in setJointControl.");
PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
return NULL;
}
static PyObject *
pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
{

2
src/BulletDynamics/Character/btCharacterControllerInterface.h
View File

@ -37,7 +37,7 @@ public:
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump () = 0;
virtual void jump(const btVector3& dir = btVector3()) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;

461
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
return direction - parallelComponent(direction, normal);
}
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
{
m_upAxis = upAxis;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
setUp(up);
setStepHeight(stepHeight);
m_addedMargin = 0.02;
m_walkDirection.setValue(0,0,0);
m_walkDirection.setValue(0.0,0.0,0.0);
m_AngVel.setValue(0.0, 0.0, 0.0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_gravity = 9.8 * 3 ; // 3G acceleration.
m_gravity = 9.8 * 3.0 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
m_SetjumpSpeed = m_jumpSpeed;
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
m_currentStepOffset = 0;
m_currentStepOffset = 0.0;
m_maxPenetrationDepth = 0.2;
full_drop = false;
bounce_fix = false;
m_linearDamping = btScalar(0.0);
m_angularDamping = btScalar(0.0);
}
btKinematicCharacterController::~btKinematicCharacterController ()
@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
btScalar maxPen = btScalar(0.0);
// btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_manifoldArray.resize(0);
@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
if (!needsCollision(obj0, obj1))
continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btScalar dist = pt.getDistance();
if (dist < 0.0)
if (dist < -m_maxPenetrationDepth)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
// TODO: cause problems on slopes, not sure if it is needed
//if (dist < maxPen)
//{
// maxPen = dist;
// m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
}
//}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
start.setOrigin(m_currentPosition);
m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
m_currentPosition = m_targetPosition;
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
}
else
{
world->convexSweepTest (m_convexShape, start, end, callback);
world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
btTransform& xform = m_ghostObject->getWorldTransform();
xform.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(xform);
// fix penetration if we hit a ceiling for example
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(world))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
m_currentPosition = m_targetPosition;
if (m_verticalOffset > 0)
{
m_verticalOffset = 0.0;
m_verticalVelocity = 0.0;
m_currentStepOffset = m_stepHeight;
}
} else {
m_currentStepOffset = m_stepHeight;
m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
{
bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
return collides;
}
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
//interferes with step movement
//updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest)
if (!(start == end))
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
else
{
collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we moved only a fraction
//btScalar hitDistance;
@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
// if (!m_wasJumping)
// we moved whole way
m_currentPosition = m_targetPosition;
}
m_currentPosition = m_targetPosition;
// if (callback.m_closestHitFraction == 0.f)
// break;
@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
btVector3 gravity_drop = m_up * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if (m_verticalVelocity > 0.0)
return;
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@ -471,30 +529,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
if (bounce_fix == true)
has_hit = callback.hasHit() || callback2.hasHit();
has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
has_hit = callback2.hasHit();
has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
downVelocity = stepHeight;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
if (callback.hasHit() || runonce == true)
if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we dropped a fraction of the height -> hit floor
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@ -513,10 +574,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@ -528,7 +589,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_wasJumping = false;
} else {
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
@ -579,21 +640,63 @@ btScalar timeInterval
m_velocityTimeInterval += timeInterval;
}
void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
{
m_AngVel = velocity;
}
const btVector3& btKinematicCharacterController::getAngularVelocity() const
{
return m_AngVel;
}
void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
{
m_walkDirection = velocity;
// HACK: if we are moving in the direction of the up, treat it as a jump :(
if (m_walkDirection.length2() > 0)
{
btVector3 w = velocity.normalized();
btScalar c = w.dot(m_up);
if (c != 0)
{
//there is a component in walkdirection for vertical velocity
btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
m_walkDirection -= upComponent;
m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
if (c > 0.0f)
{
m_wasJumping = true;
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
}
}
}
else
m_verticalVelocity = 0.0f;
}
btVector3 btKinematicCharacterController::getLinearVelocity() const
{
return m_walkDirection + (m_verticalVelocity * m_up);
}
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
{
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
}
void btKinematicCharacterController::warp (const btVector3& origin)
@ -607,62 +710,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration (collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}
#include <stdio.h>
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
if (m_AngVel.length2() > 0.0f)
{
m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
}
// integrate for angular velocity
if (m_AngVel.length2() > 0.0f)
{
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
btQuaternion orn = rot * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
}
// quick check...
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
//btVector3 lvel = m_walkDirection;
btScalar c = 0.0f;
if (m_walkDirection.length2() > 0)
{
// apply damping
m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
}
m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
btTransform xform;
xform = m_ghostObject->getWorldTransform ();
xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp (collisionWorld);
stepUp(collisionWorld);
//todo: Experimenting with behavior of controller when it hits a ceiling..
//bool hitUp = stepUp (collisionWorld);
//if (hitUp)
//{
// m_verticalVelocity -= m_gravity * dt;
// if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
// {
// m_verticalVelocity = m_jumpSpeed;
// }
// if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
// {
// m_verticalVelocity = -btFabs(m_fallSpeed);
// }
// m_verticalOffset = m_verticalVelocity * dt;
// xform = m_ghostObject->getWorldTransform();
//}
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
stepDown (collisionWorld, dt);
//todo: Experimenting with max jump height
//if (m_wasJumping)
//{
// btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
// if (ds > m_maxJumpHeight)
// {
// // substract the overshoot
// m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
// // max height was reached, so potential energy is at max
// // and kinematic energy is 0, thus velocity is 0.
// if (m_verticalVelocity > 0.0)
// m_verticalVelocity = 0.0;
// }
//}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const
return onGround();
}
void btKinematicCharacterController::jump ()
void btKinematicCharacterController::jump(const btVector3& v)
{
if (!canJump())
return;
m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
#if 0
currently no jumping.
btTransform xform;
@ -727,14 +898,16 @@ void btKinematicCharacterController::jump ()
#endif
}
void btKinematicCharacterController::setGravity(btScalar gravity)
void btKinematicCharacterController::setGravity(const btVector3& gravity)
{
m_gravity = gravity;
if (gravity.length2() > 0) setUpVector(-gravity);
m_gravity = gravity.length();
}
btScalar btKinematicCharacterController::getGravity() const
btVector3 btKinematicCharacterController::getGravity() const
{
return m_gravity;
return -m_gravity * m_up;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
return m_maxSlopeRadians;
}
bool btKinematicCharacterController::onGround () const
void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
{
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
m_maxPenetrationDepth = d;
}
btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
{
return m_maxPenetrationDepth;
}
bool btKinematicCharacterController::onGround () const
{
return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
}
void btKinematicCharacterController::setStepHeight(btScalar h)
{
m_stepHeight = h;
}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}
void btKinematicCharacterController::setUp(const btVector3& up)
{
if (up.length2() > 0 && m_gravity > 0.0f)
{
setGravity(-m_gravity * up.normalized());
return;
}
setUpVector(up);
}
void btKinematicCharacterController::setUpVector(const btVector3& up)
{
if (m_up == up)
return;
btVector3 u = m_up;
if (up.length2() > 0)
m_up = up.normalized();
else
m_up = btVector3(0.0, 0.0, 0.0);
if (!m_ghostObject) return;
btQuaternion rot = getRotation(m_up, u);
//set orientation with new up
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion orn = rot.inverse() * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
}
btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
{
if (v0.length2() == 0.0f || v1.length2() == 0.0f)
{
btQuaternion q;
return q;
}
return shortestArcQuatNormalize2(v0, v1);
}

60
src/BulletDynamics/Character/btKinematicCharacterController.h
View File

@ -43,10 +43,12 @@ protected:
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
btScalar m_maxPenetrationDepth;
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_SetjumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@ -61,24 +63,34 @@ protected:
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
btVector3 m_AngVel;
btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
btQuaternion m_currentOrientation;
btQuaternion m_targetOrientation;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
int m_upAxis;
btVector3 m_up;
btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
@ -94,11 +106,18 @@ protected:
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
void setUpVector(const btVector3& up);
btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
@ -112,14 +131,9 @@ public:
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
void setUpAxis (int axis)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
}
void setUp(const btVector3& up);
const btVector3& getUp() { return m_up; }
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
@ -136,27 +150,47 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
virtual void setAngularVelocity(const btVector3& velocity);
virtual const btVector3& getAngularVelocity() const;
virtual void setLinearVelocity(const btVector3& velocity);
virtual btVector3 getLinearVelocity() const;
void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getLinearDamping() const { return m_linearDamping; }
void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getAngularDamping() const { return m_angularDamping; }
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
void setStepHeight(btScalar h);
btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
void jump(const btVector3& v = btVector3());
void setGravity(btScalar gravity);
btScalar getGravity() const;
void applyImpulse(const btVector3& v) { jump(v); }
void setGravity(const btVector3& gravity);
btVector3 getGravity() const;
/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
void setMaxPenetrationDepth(btScalar d);
btScalar getMaxPenetrationDepth() const;
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{

42
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -299,7 +299,7 @@ void btMultiBody::setupPlanar(int i,
m_links[i].m_eVector = parentComToThisComOffset;
//
static btVector3 vecNonParallelToRotAxis(1, 0, 0);
btVector3 vecNonParallelToRotAxis(1, 0, 0);
if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
vecNonParallelToRotAxis.setValue(0, 1, 0);
//
@ -714,15 +714,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
btScalar * Y = &scratch_r[0];
//
//aux variables
static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
static btSpatialTransformationMatrix fromWorld;
btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
btSpatialTransformationMatrix fromWorld;
fromWorld.m_trnVec.setZero();
/////////////////
@ -919,8 +919,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::eSpherical:
case btMultibodyLink::ePlanar:
{
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
//unroll the loop?
for(int row = 0; row < 3; ++row)
@ -1323,11 +1323,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
btScalar * Y = r_ptr;
////////////////
//aux variables
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
static btSpatialTransformationMatrix fromParent;
btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
btSpatialTransformationMatrix fromParent;
/////////////////
// First 'upward' loop.
@ -1522,8 +1522,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
//
static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
pBaseQuat[0] = baseQuat.x();
pBaseQuat[1] = baseQuat.y();
@ -1557,8 +1557,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
case btMultibodyLink::eSpherical:
{
static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
break;