Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source
This commit is contained in:
Erwin Coumans
2019-03-20 15:11:03 -07:00
parent f725d1201d 28148eb1ed
commit 94d8ee74f7
17 changed files with 65 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/DynamicControlDemo/MotorDemo.cpp
View File

@@ -18,6 +18,8 @@ subject to the following restrictions:
#include "LinearMath/btIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "MotorDemo.h" #include "MotorDemo.h"
#include <cmath>
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
class btBroadphaseInterface; class btBroadphaseInterface;
class btCollisionShape; class btCollisionShape;
@@ -158,8 +160,8 @@ public:
for (i = 0; i < NUM_LEGS; i++) for (i = 0; i < NUM_LEGS; i++)
{ {
float fAngle = 2 * M_PI * i / NUM_LEGS; float fAngle = 2 * M_PI * i / NUM_LEGS;
float fSin = sin(fAngle); float fSin = std::sin(fAngle);
float fCos = cos(fAngle); float fCos = std::cos(fAngle);
transform.setIdentity(); transform.setIdentity();
btVector3 vBoneOrigin = btVector3(btScalar(fCos * (fBodySize + 0.5 * fLegLength)), btScalar(fHeight), btScalar(fSin * (fBodySize + 0.5 * fLegLength))); btVector3 vBoneOrigin = btVector3(btScalar(fCos * (fBodySize + 0.5 * fLegLength)), btScalar(fHeight), btScalar(fSin * (fBodySize + 0.5 * fLegLength)));
@@ -197,8 +199,8 @@ public:
for (i = 0; i < NUM_LEGS; i++) for (i = 0; i < NUM_LEGS; i++)
{ {
float fAngle = 2 * M_PI * i / NUM_LEGS; float fAngle = 2 * M_PI * i / NUM_LEGS;
float fSin = sin(fAngle); float fSin = std::sin(fAngle);
float fCos = cos(fAngle); float fCos = std::cos(fAngle);
// hip joints // hip joints
localA.setIdentity(); localA.setIdentity();

8
examples/Evolution/NN3DWalkers.cpp
View File

@@ -15,6 +15,8 @@ subject to the following restrictions:
#include "NN3DWalkers.h" #include "NN3DWalkers.h"
#include <cmath>
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/btIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
@@ -282,8 +284,8 @@ public:
for (i = 0; i < NUM_LEGS; i++) for (i = 0; i < NUM_LEGS; i++)
{ {
float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
float footYUnitPosition = sin(footAngle); // y position of the leg on the unit circle float footYUnitPosition = std::sin(footAngle); // y position of the leg on the unit circle
float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle float footXUnitPosition = std::cos(footAngle); // x position of the leg on the unit circle
transform.setIdentity(); transform.setIdentity();
btVector3 legCOM = btVector3(btScalar(footXUnitPosition * (gRootBodyRadius + 0.5 * gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition * (gRootBodyRadius + 0.5 * gLegLength))); btVector3 legCOM = btVector3(btScalar(footXUnitPosition * (gRootBodyRadius + 0.5 * gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition * (gRootBodyRadius + 0.5 * gLegLength)));
@@ -1017,7 +1019,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick)
} }
// apply the activation function // apply the activation function
targetAngle = (tanh(targetAngle) + 1.0f) * 0.5f; targetAngle = (std::tanh(targetAngle) + 1.0f) * 0.5f;
} }
btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit()); btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
btScalar currentAngle = hingeC->getHingeAngle(); btScalar currentAngle = hingeC->getHingeAngle();

16
examples/ExampleBrowser/GL_ShapeDrawer.cpp
View File

@@ -48,6 +48,8 @@ subject to the following restrictions:
#include <stdio.h> //printf debugging #include <stdio.h> //printf debugging
#include <cmath>
#if defined(BT_USE_DOUBLE_PRECISION) #if defined(BT_USE_DOUBLE_PRECISION)
#define btglLoadMatrix glLoadMatrixd #define btglLoadMatrix glLoadMatrixd
#define btglMultMatrix glMultMatrixd #define btglMultMatrix glMultMatrixd
@@ -149,19 +151,19 @@ void GL_ShapeDrawer::drawSphere(btScalar radius, int lats, int longs)
for (i = 0; i <= lats; i++) for (i = 0; i <= lats; i++)
{ {
btScalar lat0 = SIMD_PI * (-btScalar(0.5) + (btScalar)(i - 1) / lats); btScalar lat0 = SIMD_PI * (-btScalar(0.5) + (btScalar)(i - 1) / lats);
btScalar z0 = radius * sin(lat0); btScalar z0 = radius * std::sin(lat0);
btScalar zr0 = radius * cos(lat0); btScalar zr0 = radius * std::cos(lat0);
btScalar lat1 = SIMD_PI * (-btScalar(0.5) + (btScalar)i / lats); btScalar lat1 = SIMD_PI * (-btScalar(0.5) + (btScalar)i / lats);
btScalar z1 = radius * sin(lat1); btScalar z1 = radius * std::sin(lat1);
btScalar zr1 = radius * cos(lat1); btScalar zr1 = radius * std::cos(lat1);
glBegin(GL_QUAD_STRIP); glBegin(GL_QUAD_STRIP);
for (j = 0; j <= longs; j++) for (j = 0; j <= longs; j++)
{ {
btScalar lng = 2 * SIMD_PI * (btScalar)(j - 1) / longs; btScalar lng = 2 * SIMD_PI * (btScalar)(j - 1) / longs;
btScalar x = cos(lng); btScalar x = std::cos(lng);
btScalar y = sin(lng); btScalar y = std::sin(lng);
glNormal3f(x * zr1, y * zr1, z1); glNormal3f(x * zr1, y * zr1, z1);
glVertex3f(x * zr1, y * zr1, z1); glVertex3f(x * zr1, y * zr1, z1);
glNormal3f(x * zr0, y * zr0, z0); glNormal3f(x * zr0, y * zr0, z0);
@@ -906,4 +908,4 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
glDisable(GL_CULL_FACE); glDisable(GL_CULL_FACE);
drawSceneInternal(dynamicsWorld, 0, cameraUpAxis); drawSceneInternal(dynamicsWorld, 0, cameraUpAxis);
} }
} }

5
examples/ExtendedTutorials/MultiPendulum.cpp
View File

@@ -15,8 +15,9 @@
#include "MultiPendulum.h" #include "MultiPendulum.h"
#include <vector> // TODO: Should I use another data structure? #include <cmath>
#include <iterator> #include <iterator>
#include <vector> // TODO: Should I use another data structure?
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
@@ -159,7 +160,7 @@ void MultiPendulumExample::initPhysics()
m_collisionShapes.push_back(pendulumShape); m_collisionShapes.push_back(pendulumShape);
// create multi-pendulum // create multi-pendulum
createMultiPendulum(pendulumShape, floor(gPendulaQty), position, createMultiPendulum(pendulumShape, std::floor(gPendulaQty), position,
gInitialPendulumLength, pendulumMass); gInitialPendulumLength, pendulumMass);
} }

5
examples/ExtendedTutorials/NewtonsCradle.cpp
View File

@@ -15,8 +15,9 @@
#include "NewtonsCradle.h" #include "NewtonsCradle.h"
#include <vector> // TODO: Should I use another data structure? #include <cmath>
#include <iterator> #include <iterator>
#include <vector> // TODO: Should I use another data structure?
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
@@ -158,7 +159,7 @@ void NewtonsCradleExample::initPhysics()
btSphereShape* pendulumShape = new btSphereShape(gSphereRadius); btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
m_collisionShapes.push_back(pendulumShape); m_collisionShapes.push_back(pendulumShape);
for (int i = 0; i < floor(gPendulaQty); i++) for (int i = 0; i < std::floor(gPendulaQty); i++)
{ {
// create pendulum // create pendulum
createPendulum(pendulumShape, position, gInitialPendulumLength, pendulumMass); createPendulum(pendulumShape, position, gInitialPendulumLength, pendulumMass);

5
examples/ExtendedTutorials/NewtonsRopeCradle.cpp
View File

@@ -15,8 +15,9 @@
#include "NewtonsRopeCradle.h" #include "NewtonsRopeCradle.h"
#include <vector> // TODO: Should I use another data structure? #include <cmath>
#include <iterator> #include <iterator>
#include <vector> // TODO: Should I use another data structure?
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
@@ -204,7 +205,7 @@ void NewtonsRopeCradleExample::initPhysics()
btSphereShape* pendulumShape = new btSphereShape(gSphereRadius); btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
m_collisionShapes.push_back(pendulumShape); m_collisionShapes.push_back(pendulumShape);
for (int i = 0; i < floor(gPendulaQty); i++) for (int i = 0; i < std::floor(gPendulaQty); i++)
{ {
// create pendulum // create pendulum
createRopePendulum(pendulumShape, position, orientation, gInitialPendulumWidth, createRopePendulum(pendulumShape, position, orientation, gInitialPendulumWidth,

3
examples/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -22,6 +22,7 @@ subject to the following restrictions:
#include <stdio.h> //printf debugging #include <stdio.h> //printf debugging
#include <algorithm> #include <algorithm>
#include <cmath>
static btScalar gSliderStackRows = 1.0f; static btScalar gSliderStackRows = 1.0f;
static btScalar gSliderStackColumns = 1.0f; static btScalar gSliderStackColumns = 1.0f;
@@ -107,7 +108,7 @@ public:
// update ground // update ground
const float cyclesPerSecond = 1.0f; const float cyclesPerSecond = 1.0f;
m_groundMovePhase += cyclesPerSecond * deltaTime; m_groundMovePhase += cyclesPerSecond * deltaTime;
m_groundMovePhase -= floor(m_groundMovePhase); // keep phase between 0 and 1 m_groundMovePhase -= std::floor(m_groundMovePhase); // keep phase between 0 and 1
btTransform xf = m_groundStartXf; btTransform xf = m_groundStartXf;
float gndHOffset = btSin(m_groundMovePhase * SIMD_2_PI) * gSliderGroundHorizontalAmplitude; float gndHOffset = btSin(m_groundMovePhase * SIMD_2_PI) * gSliderGroundHorizontalAmplitude;
float gndHVel = btCos(m_groundMovePhase * SIMD_2_PI) * gSliderGroundHorizontalAmplitude * cyclesPerSecond * SIMD_2_PI; // d(gndHOffset)/dt float gndHVel = btCos(m_groundMovePhase * SIMD_2_PI) * gSliderGroundHorizontalAmplitude * cyclesPerSecond * SIMD_2_PI; // d(gndHOffset)/dt

2
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1082,7 +1082,7 @@ B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemor
int numLinks = status->m_sendActualStateArgs.m_numLinks; int numLinks = status->m_sendActualStateArgs.m_numLinks;
b3Assert(linkIndex < numLinks); b3Assert(linkIndex < numLinks);
if ((bodyIndex >= 0) && (linkIndex >= 0) && linkIndex < numLinks) if (status->m_sendActualStateArgs.m_stateDetails != NULL && (bodyIndex >= 0) && (linkIndex >= 0) && linkIndex < numLinks)
{ {
b3Transform wlf, com, inertial; b3Transform wlf, com, inertial;

11
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1613,6 +1613,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_physicsDeltaTime; btScalar m_physicsDeltaTime;
btScalar m_numSimulationSubSteps; btScalar m_numSimulationSubSteps;
btScalar m_simulationTimestamp;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks; btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies; b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
b3HashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers; b3HashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
@@ -7608,10 +7609,12 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
if (m_data->m_numSimulationSubSteps > 0) if (m_data->m_numSimulationSubSteps > 0)
{ {
numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
m_data->m_simulationTimestamp += deltaTimeScaled;
} }
else else
{ {
numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0); numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
m_data->m_simulationTimestamp += deltaTimeScaled;
} }
if (numSteps > 0) if (numSteps > 0)
@@ -8161,6 +8164,7 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration; serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration;
serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
serverCmd.m_simulationParameterResultArgs.m_simulationTimestamp = m_data->m_simulationTimestamp;
serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop; serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex; serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;
@@ -11718,7 +11722,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const
gSubStep = m_data->m_physicsDeltaTime; gSubStep = m_data->m_physicsDeltaTime;
} }
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec * simTimeScalingFactor, maxSteps, gSubStep); btScalar deltaTimeScaled = dtInSec * simTimeScalingFactor;
int numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, maxSteps, gSubStep);
m_data->m_simulationTimestamp += deltaTimeScaled;
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps) if (numSteps)
@@ -11797,7 +11803,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
{ {
//clean up all data //clean up all data
m_data->m_cachedVUrdfisualShapes.clear(); m_data->m_simulationTimestamp = 0;
m_data->m_cachedVUrdfisualShapes.clear();
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (m_data && m_data->m_dynamicsWorld) if (m_data && m_data->m_dynamicsWorld)

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -900,6 +900,7 @@ struct b3PluginArguments
struct b3PhysicsSimulationParameters struct b3PhysicsSimulationParameters
{ {
double m_deltaTime; double m_deltaTime;
double m_simulationTimestamp; // Output only timestamp of simulation.
double m_gravityAcceleration[3]; double m_gravityAcceleration[3];
int m_numSimulationSubSteps; int m_numSimulationSubSteps;
int m_numSolverIterations; int m_numSolverIterations;

4
examples/ThirdPartyLibs/BussIK/Misc.cpp
View File

@@ -21,6 +21,8 @@ subject to the following restrictions:
*/ */
#include <math.h> #include <math.h>
#include <cmath>
#include "LinearR3.h" #include "LinearR3.h"
#if 0 #if 0
@@ -121,7 +123,7 @@ float unit(float vin[3], float vout[3])
if (dist > 0.0) if (dist > 0.0)
{ {
dist = sqrt(dist); dist = std::sqrt(dist);
f = 1. / dist; f = 1. / dist;
vout[0] = f * vin[0]; vout[0] = f * vin[0];
vout[1] = f * vin[1]; vout[1] = f * vin[1];

12
examples/ThirdPartyLibs/Gwen/BaseRender.cpp
View File

@@ -11,6 +11,8 @@
#include <math.h> #include <math.h>
#include <cmath>
namespace Gwen namespace Gwen
{ {
namespace Renderer namespace Renderer
@@ -87,16 +89,16 @@ void Base::Translate(int& x, int& y)
x += m_RenderOffset.x; x += m_RenderOffset.x;
y += m_RenderOffset.y; y += m_RenderOffset.y;
x = ceil(((float)x) * m_fScale); x = std::ceil(((float)x) * m_fScale);
y = ceil(((float)y) * m_fScale); y = std::ceil(((float)y) * m_fScale);
} }
void Base::Translate(Gwen::Rect& rect) void Base::Translate(Gwen::Rect& rect)
{ {
Translate(rect.x, rect.y); Translate(rect.x, rect.y);
rect.w = ceil(((float)rect.w) * m_fScale); rect.w = std::ceil(((float)rect.w) * m_fScale);
rect.h = ceil(((float)rect.h) * m_fScale); rect.h = std::ceil(((float)rect.h) * m_fScale);
} }
void Gwen::Renderer::Base::SetClipRegion(Gwen::Rect rect) void Gwen::Renderer::Base::SetClipRegion(Gwen::Rect rect)
@@ -214,4 +216,4 @@ Gwen::Point Base::MeasureText(Gwen::Font* pFont, const Gwen::UnicodeString& text
return p; return p;
} }
} // namespace Renderer } // namespace Renderer
} // namespace Gwen } // namespace Gwen

5
examples/ThirdPartyLibs/Gwen/Controls/Slider.cpp
View File

@@ -5,6 +5,7 @@
*/ */
#include <math.h> #include <math.h>
#include <cmath>
#include "Gwen/Controls/Slider.h" #include "Gwen/Controls/Slider.h"
using namespace Gwen; using namespace Gwen;
@@ -68,7 +69,7 @@ void Slider::SetValueInternal(float val)
{ {
if (m_bClampToNotches) if (m_bClampToNotches)
{ {
val = floor((val * (float)m_iNumNotches) + 0.5f); val = std::floor((val * (float)m_iNumNotches) + 0.5f);
val /= (float)m_iNumNotches; val /= (float)m_iNumNotches;
} }
@@ -98,4 +99,4 @@ void Slider::RenderFocus(Gwen::Skin::Base* skin)
if (!IsTabable()) return; if (!IsTabable()) return;
skin->DrawKeyboardHighlight(this, GetRenderBounds(), 0); skin->DrawKeyboardHighlight(this, GetRenderBounds(), 0);
} }

2
examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py
View File

@@ -316,7 +316,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
if test: if test:
urdf_pattern = os.path.join(self._urdfRoot, 'random_urdfs/*0/*.urdf') urdf_pattern = os.path.join(self._urdfRoot, 'random_urdfs/*0/*.urdf')
else: else:
urdf_pattern = os.path.join(self._urdfRoot, 'random_urdfs/*[^0]/*.urdf') urdf_pattern = os.path.join(self._urdfRoot, 'random_urdfs/*[1-9]/*.urdf')
found_object_directories = glob.glob(urdf_pattern) found_object_directories = glob.glob(urdf_pattern)
total_num_objects = len(found_object_directories) total_num_objects = len(found_object_directories)
selected_objects = np.random.choice(np.arange(total_num_objects), selected_objects = np.random.choice(np.arange(total_num_objects),

4
src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
View File

@@ -22,6 +22,8 @@ subject to the following restrictions:
#include <string.h> //for memset #include <string.h> //for memset
#include <cmath>
const int kNoMerge = -1; const int kNoMerge = -1;
bool btBatchedConstraints::s_debugDrawBatches = false; bool btBatchedConstraints::s_debugDrawBatches = false;
@@ -520,7 +522,7 @@ static void writeGrainSizes(btBatchedConstraints* bc)
{ {
const Range& phase = bc->m_phases[iPhase]; const Range& phase = bc->m_phases[iPhase];
int numBatches = phase.end - phase.begin; int numBatches = phase.end - phase.begin;
float grainSize = floor((0.25f * numBatches / float(numThreads)) + 0.0f); float grainSize = std::floor((0.25f * numBatches / float(numThreads)) + 0.0f);
bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize)); bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize));
} }
} }

5
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -19,6 +19,7 @@ Written by: Marcus Hennix
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMinMax.h" #include "LinearMath/btMinMax.h"
#include <cmath>
#include <new> #include <new>
//#define CONETWIST_USE_OBSOLETE_SOLVER true //#define CONETWIST_USE_OBSOLETE_SOLVER true
@@ -842,7 +843,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm; btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2); swingLimit = std::sqrt(swingLimit2);
} }
// test! // test!
@@ -887,7 +888,7 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm; btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2); swingLimit = std::sqrt(swingLimit2);
} }
// convert into point in constraint space: // convert into point in constraint space:

5
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -40,6 +40,7 @@ http://gimpact.sf.net
#include "btGeneric6DofSpring2Constraint.h" #include "btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#include <cmath>
#include <new> #include <new>
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder) btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
@@ -845,7 +846,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (m_rbA.getInvMass() == 0) m = mB; else if (m_rbA.getInvMass() == 0) m = mB; else
if (m_rbB.getInvMass() == 0) m = mA; else if (m_rbB.getInvMass() == 0) m = mA; else
m = mA*mB / (mA + mB); m = mA*mB / (mA + mB);
btScalar angularfreq = sqrt(ks / m); btScalar angularfreq = btSqrt(ks / m);
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt) if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
@@ -1085,7 +1086,7 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr
btScalar target = targetOrg + SIMD_PI; btScalar target = targetOrg + SIMD_PI;
if (1) if (1)
{ {
btScalar m = target - SIMD_2_PI * floor(target / SIMD_2_PI); btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
// handle boundary cases resulted from floating-point cut off: // handle boundary cases resulted from floating-point cut off:
{ {
if (m >= SIMD_2_PI) if (m >= SIMD_2_PI)