plumb URDF/SDF audio_source into PhysicsServerCommandProcessor, allow to play sounds on collision !
See also https://youtu.be/eppOjTfx5Jg for a first test, and this modified URDF how to add sounds: https://github.com/bulletphysics/bullet3/blob/master/data/plane_with_collision_audio.urdf Add the --audio flag to enable sound in pybullet/Bullet-C-API
This commit is contained in:
37
data/plane_with_collision_audio.urdf
Normal file
37
data/plane_with_collision_audio.urdf
Normal file
@@ -0,0 +1,37 @@
|
|||||||
|
<?xml version="0.0" ?>
|
||||||
|
<robot name="plane">
|
||||||
|
<link name="planeLink">
|
||||||
|
<audio_source filename="wav/cardboardbox0.wav">
|
||||||
|
<collision_force_threshold>0.4</collision_force_threshold>
|
||||||
|
<attack_rate>0.01</attack_rate>
|
||||||
|
<decay_rate>0.0001</decay_rate>
|
||||||
|
<sustain_level>0.5</sustain_level>
|
||||||
|
<release_rate>0.005</release_rate>
|
||||||
|
<gain>.5</gain>
|
||||||
|
</audio_source>
|
||||||
|
<contact>
|
||||||
|
<lateral_friction value="1"/>
|
||||||
|
</contact>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value=".0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="plane.obj" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -5"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="30 30 10"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
|
|
||||||
@@ -48,7 +48,7 @@
|
|||||||
//quick test for file import, @todo(erwincoumans) make it more general and add other file formats
|
//quick test for file import, @todo(erwincoumans) make it more general and add other file formats
|
||||||
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
|
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
|
||||||
#include "../Importers/ImportBullet/SerializeSetup.h"
|
#include "../Importers/ImportBullet/SerializeSetup.h"
|
||||||
|
#include "../Utils/b3HashString.h"
|
||||||
#include "Bullet3Common/b3HashMap.h"
|
#include "Bullet3Common/b3HashMap.h"
|
||||||
|
|
||||||
struct GL3TexLoader : public MyTextureLoader
|
struct GL3TexLoader : public MyTextureLoader
|
||||||
|
|||||||
@@ -1134,9 +1134,9 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
|
bool BulletURDFImporter::getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const
|
||||||
{
|
{
|
||||||
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
|
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdflinkIndex);
|
||||||
if (linkPtr)
|
if (linkPtr)
|
||||||
{
|
{
|
||||||
const UrdfLink* link = *linkPtr;
|
const UrdfLink* link = *linkPtr;
|
||||||
@@ -1146,6 +1146,22 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool BulletURDFImporter::getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const
|
||||||
|
{
|
||||||
|
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
|
||||||
|
if (linkPtr)
|
||||||
|
{
|
||||||
|
const UrdfLink* link = *linkPtr;
|
||||||
|
if (link->m_audioSource.m_flags & SDFAudioSource::SDFAudioSourceValid)
|
||||||
|
{
|
||||||
|
audioSource = link->m_audioSource;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
||||||
{
|
{
|
||||||
if (m_data->m_customVisualShapesConverter)
|
if (m_data->m_customVisualShapesConverter)
|
||||||
|
|||||||
@@ -41,8 +41,10 @@ public:
|
|||||||
|
|
||||||
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
|
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
|
||||||
|
|
||||||
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
|
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const;
|
||||||
|
|
||||||
|
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
|
||||||
|
|
||||||
virtual std::string getJointName(int linkIndex) const;
|
virtual std::string getJointName(int linkIndex) const;
|
||||||
|
|
||||||
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
||||||
|
|||||||
@@ -4,6 +4,7 @@
|
|||||||
|
|
||||||
#include "MultiBodyCreationInterface.h"
|
#include "MultiBodyCreationInterface.h"
|
||||||
#include "LinearMath/btAlignedObjectArray.h"
|
#include "LinearMath/btAlignedObjectArray.h"
|
||||||
|
#include "LinearMath/btHashMap.h"
|
||||||
|
|
||||||
struct GUIHelperInterface;
|
struct GUIHelperInterface;
|
||||||
class btMultiBody;
|
class btMultiBody;
|
||||||
@@ -28,7 +29,6 @@ protected:
|
|||||||
|
|
||||||
struct GUIHelperInterface* m_guiHelper;
|
struct GUIHelperInterface* m_guiHelper;
|
||||||
|
|
||||||
|
|
||||||
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
|
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|||||||
46
examples/Importers/ImportURDFDemo/SDFAudioTypes.h
Normal file
46
examples/Importers/ImportURDFDemo/SDFAudioTypes.h
Normal file
@@ -0,0 +1,46 @@
|
|||||||
|
#ifndef SDF_AUDIO_TYPES_H
|
||||||
|
#define SDF_AUDIO_TYPES_H
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
///See audio_source element in http://sdformat.org/spec?ver=1.6&elem=link
|
||||||
|
struct SDFAudioSource
|
||||||
|
{
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
SDFAudioSourceValid=1,
|
||||||
|
SDFAudioSourceLooping=2,
|
||||||
|
};
|
||||||
|
|
||||||
|
int m_flags; //repeat mode (0 = no repeat, 1 = loop forever)
|
||||||
|
|
||||||
|
std::string m_uri; //media filename of the sound, .wav file
|
||||||
|
double m_pitch; //1 = regular rate, -1 play in reverse
|
||||||
|
double m_gain; //normalized volume in range [0..1] where 0 is silent, 1 is most loud
|
||||||
|
|
||||||
|
double m_attackRate;
|
||||||
|
double m_decayRate;
|
||||||
|
double m_sustainLevel;
|
||||||
|
double m_releaseRate;
|
||||||
|
|
||||||
|
double m_collisionForceThreshold; //force that will trigger the audio, in Newton. If < 0, audio source is invalid
|
||||||
|
|
||||||
|
int m_userIndex;
|
||||||
|
|
||||||
|
SDFAudioSource()
|
||||||
|
: m_flags(0),
|
||||||
|
m_pitch(1),
|
||||||
|
m_gain(1),
|
||||||
|
m_attackRate(0.0001),
|
||||||
|
m_decayRate(0.00001),
|
||||||
|
m_sustainLevel(0.5),
|
||||||
|
m_releaseRate(0.0005),
|
||||||
|
m_collisionForceThreshold(0.5),
|
||||||
|
m_userIndex(-1)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif //SDF_AUDIO_TYPES_H
|
||||||
@@ -5,7 +5,7 @@
|
|||||||
#include "LinearMath/btAlignedObjectArray.h"
|
#include "LinearMath/btAlignedObjectArray.h"
|
||||||
#include "LinearMath/btTransform.h"
|
#include "LinearMath/btTransform.h"
|
||||||
#include "URDFJointTypes.h"
|
#include "URDFJointTypes.h"
|
||||||
|
#include "SDFAudioTypes.h"
|
||||||
|
|
||||||
class URDFImporterInterface
|
class URDFImporterInterface
|
||||||
{
|
{
|
||||||
@@ -40,6 +40,8 @@ public:
|
|||||||
///this API will likely change, don't override it!
|
///this API will likely change, don't override it!
|
||||||
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
|
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
|
||||||
|
|
||||||
|
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const { return false;}
|
||||||
|
|
||||||
virtual std::string getJointName(int linkIndex) const = 0;
|
virtual std::string getJointName(int linkIndex) const = 0;
|
||||||
|
|
||||||
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
|
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
|
||||||
|
|||||||
@@ -663,6 +663,82 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
//optional 'audio_source' parameters
|
||||||
|
//modified version of SDF audio_source specification in //http://sdformat.org/spec?ver=1.6&elem=link
|
||||||
|
#if 0
|
||||||
|
<audio_source>
|
||||||
|
<uri>file://media/audio/cheer.mp3</uri>
|
||||||
|
<pitch>2.0</pitch>
|
||||||
|
<gain>1.0</gain>
|
||||||
|
<loop>false</loop>
|
||||||
|
<contact>
|
||||||
|
<collision>collision</collision>
|
||||||
|
</contact>
|
||||||
|
</audio_source>
|
||||||
|
#endif
|
||||||
|
TiXmlElement* ci = config->FirstChildElement("audio_source");
|
||||||
|
if (ci)
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_flags |= SDFAudioSource::SDFAudioSourceValid;
|
||||||
|
|
||||||
|
const char* fn = ci->Attribute("filename");
|
||||||
|
if (fn)
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_uri = fn;
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
if (TiXmlElement* filename_xml = ci->FirstChildElement("uri"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_uri = filename_xml->GetText();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (TiXmlElement* pitch_xml = ci->FirstChildElement("pitch"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_pitch = urdfLexicalCast<double>(pitch_xml->GetText());
|
||||||
|
}
|
||||||
|
if (TiXmlElement* gain_xml = ci->FirstChildElement("gain"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_gain = urdfLexicalCast<double>(gain_xml->GetText());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TiXmlElement* attack_rate_xml = ci->FirstChildElement("attack_rate"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_attackRate = urdfLexicalCast<double>(attack_rate_xml->GetText());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TiXmlElement* decay_rate_xml = ci->FirstChildElement("decay_rate"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_decayRate = urdfLexicalCast<double>(decay_rate_xml->GetText());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TiXmlElement* sustain_level_xml = ci->FirstChildElement("sustain_level"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_sustainLevel = urdfLexicalCast<double>(sustain_level_xml->GetText());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TiXmlElement* release_rate_xml = ci->FirstChildElement("release_rate"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_releaseRate = urdfLexicalCast<double>(release_rate_xml->GetText());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TiXmlElement* loop_xml = ci->FirstChildElement("loop"))
|
||||||
|
{
|
||||||
|
std::string looptxt = loop_xml->GetText();
|
||||||
|
if (looptxt == "true")
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_flags |= SDFAudioSource::SDFAudioSourceLooping;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (TiXmlElement* forceThreshold_xml = ci->FirstChildElement("collision_force_threshold"))
|
||||||
|
{
|
||||||
|
link.m_audioSource.m_collisionForceThreshold= urdfLexicalCast<double>(forceThreshold_xml->GetText());
|
||||||
|
}
|
||||||
|
//todo: see other audio_source children
|
||||||
|
//pitch, gain, loop, contact
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
{
|
{
|
||||||
//optional 'contact' parameters
|
//optional 'contact' parameters
|
||||||
TiXmlElement* ci = config->FirstChildElement("contact");
|
TiXmlElement* ci = config->FirstChildElement("contact");
|
||||||
|
|||||||
@@ -5,6 +5,7 @@
|
|||||||
#include "LinearMath/btAlignedObjectArray.h"
|
#include "LinearMath/btAlignedObjectArray.h"
|
||||||
#include "LinearMath/btHashMap.h"
|
#include "LinearMath/btHashMap.h"
|
||||||
#include "URDFJointTypes.h"
|
#include "URDFJointTypes.h"
|
||||||
|
#include "SDFAudioTypes.h"
|
||||||
|
|
||||||
#define btArray btAlignedObjectArray
|
#define btArray btAlignedObjectArray
|
||||||
#include <string>
|
#include <string>
|
||||||
@@ -140,6 +141,8 @@ struct UrdfLink
|
|||||||
|
|
||||||
URDFLinkContactInfo m_contactInfo;
|
URDFLinkContactInfo m_contactInfo;
|
||||||
|
|
||||||
|
SDFAudioSource m_audioSource;
|
||||||
|
|
||||||
UrdfLink()
|
UrdfLink()
|
||||||
:m_parentLink(0),
|
:m_parentLink(0),
|
||||||
m_parentJoint(0)
|
m_parentJoint(0)
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
#include "b3RobotSimulatorClientAPI.h"
|
#include "b3RobotSimulatorClientAPI.h"
|
||||||
|
|
||||||
#include "Bullet3Common/b3HashMap.h"
|
#include "Bullet3Common/b3HashMap.h"
|
||||||
|
#include "../Utils/b3HashString.h"
|
||||||
|
|
||||||
struct MinitaurSetupInternalData
|
struct MinitaurSetupInternalData
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -16,7 +16,7 @@ int main(int argc, char* argv[])
|
|||||||
//sim->connect(eCONNECT_UDP, "localhost", 1234);
|
//sim->connect(eCONNECT_UDP, "localhost", 1234);
|
||||||
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
|
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
|
||||||
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
|
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
|
||||||
sim->setTimeOut(10);
|
sim->setTimeOut(12345);
|
||||||
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
|
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
|
||||||
sim->syncBodies();
|
sim->syncBodies();
|
||||||
b3Scalar fixedTimeStep = 1./240.;
|
b3Scalar fixedTimeStep = 1./240.;
|
||||||
@@ -33,7 +33,7 @@ int main(int argc, char* argv[])
|
|||||||
//b3BodyInfo bodyInfo;
|
//b3BodyInfo bodyInfo;
|
||||||
//sim->getBodyInfo(blockId,&bodyInfo);
|
//sim->getBodyInfo(blockId,&bodyInfo);
|
||||||
|
|
||||||
sim->loadURDF("plane.urdf");
|
sim->loadURDF("plane_with_collision_audio.urdf");
|
||||||
|
|
||||||
b3RobotSimulatorLoadUrdfFileArgs args;
|
b3RobotSimulatorLoadUrdfFileArgs args;
|
||||||
args.m_startPosition.setValue(0,0,2);
|
args.m_startPosition.setValue(0,0,2);
|
||||||
|
|||||||
@@ -119,6 +119,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
|
|||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct InteralBodyData
|
struct InteralBodyData
|
||||||
{
|
{
|
||||||
btMultiBody* m_multiBody;
|
btMultiBody* m_multiBody;
|
||||||
@@ -128,6 +129,9 @@ struct InteralBodyData
|
|||||||
|
|
||||||
btTransform m_rootLocalInertialFrame;
|
btTransform m_rootLocalInertialFrame;
|
||||||
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
|
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
|
||||||
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
|
btHashMap<btHashInt, SDFAudioSource> m_audioSources;
|
||||||
|
#endif //B3_ENABLE_TINY_AUDIO
|
||||||
|
|
||||||
InteralBodyData()
|
InteralBodyData()
|
||||||
:m_multiBody(0),
|
:m_multiBody(0),
|
||||||
@@ -1303,7 +1307,6 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
TinyRendererVisualShapeConverter m_visualConverter;
|
TinyRendererVisualShapeConverter m_visualConverter;
|
||||||
#ifdef B3_ENABLE_TINY_AUDIO
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
b3SoundEngine m_soundEngine;
|
b3SoundEngine m_soundEngine;
|
||||||
int m_wavIds[3];
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
PhysicsServerCommandProcessorInternalData()
|
PhysicsServerCommandProcessorInternalData()
|
||||||
@@ -1513,49 +1516,66 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
|
|||||||
for (int i = 0; i < numContactManifolds; i++)
|
for (int i = 0; i < numContactManifolds; i++)
|
||||||
{
|
{
|
||||||
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
|
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
|
||||||
for (int p=0;p<manifold->getNumContacts();p++)
|
|
||||||
|
bool objHasSound[2];
|
||||||
|
objHasSound[0] = (0!=(manifold->getBody0()->getCollisionFlags() & btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER));
|
||||||
|
objHasSound[1] = (0!=(manifold->getBody1()->getCollisionFlags() & btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER));
|
||||||
|
const btCollisionObject* colObjs[2] = {manifold->getBody0(),manifold->getBody1()};
|
||||||
|
|
||||||
|
for (int ob = 0;ob<2;ob++)
|
||||||
{
|
{
|
||||||
double imp = manifold->getContactPoint(p).getAppliedImpulse();
|
if (objHasSound[ob])
|
||||||
//printf ("manifold %d, contact %d, lifeTime:%d, appliedImpulse:%f\n",i,p, manifold->getContactPoint(p).getLifeTime(),imp);
|
|
||||||
|
|
||||||
if (imp>0.4 && manifold->getContactPoint(p).getLifeTime()==1)
|
|
||||||
{
|
{
|
||||||
int soundSourceIndex = m_data->m_soundEngine.getAvailableSoundSource();
|
int uid0 = -1;
|
||||||
if (soundSourceIndex>=0)
|
int linkIndex = -2;
|
||||||
|
|
||||||
|
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(colObjs[ob]);
|
||||||
|
if (mblB && mblB->m_multiBody)
|
||||||
{
|
{
|
||||||
b3SoundMessage msg;
|
linkIndex = mblB->m_link;
|
||||||
msg.m_releaseRate = 0.0001;
|
uid0 = mblB->m_multiBody->getUserIndex2();
|
||||||
msg.m_attackRate = 1.;
|
}
|
||||||
msg.m_type = B3_SOUND_SOURCE_WAV_FILE;
|
const btRigidBody* bodyB = btRigidBody::upcast(colObjs[ob]);
|
||||||
|
if (bodyB)
|
||||||
int cardboardIndex = float(rand())/float(RAND_MAX)*2.9;
|
{
|
||||||
|
uid0 = bodyB->getUserIndex2();
|
||||||
|
linkIndex = -1;
|
||||||
|
}
|
||||||
|
|
||||||
msg.m_wavId = m_data->m_wavIds[cardboardIndex];
|
if ((uid0<0)||(linkIndex<-1))
|
||||||
|
continue;
|
||||||
|
|
||||||
float rnd = rand()% 2;
|
InternalBodyHandle* bodyHandle0 = m_data->getHandle(uid0);
|
||||||
|
SDFAudioSource* audioSrc = bodyHandle0->m_audioSources[linkIndex];
|
||||||
msg.m_frequency = 100+rnd*10.;
|
if (audioSrc==0)
|
||||||
|
continue;
|
||||||
|
|
||||||
m_data->m_soundEngine.startSound(soundSourceIndex,msg);
|
for (int p=0;p<manifold->getNumContacts();p++)
|
||||||
m_data->m_soundEngine.releaseSound(soundSourceIndex);
|
{
|
||||||
|
double imp = manifold->getContactPoint(p).getAppliedImpulse();
|
||||||
|
//printf ("manifold %d, contact %d, lifeTime:%d, appliedImpulse:%f\n",i,p, manifold->getContactPoint(p).getLifeTime(),imp);
|
||||||
|
|
||||||
|
if (imp>audioSrc->m_collisionForceThreshold && manifold->getContactPoint(p).getLifeTime()==1)
|
||||||
|
{
|
||||||
|
int soundSourceIndex = m_data->m_soundEngine.getAvailableSoundSource();
|
||||||
|
if (soundSourceIndex>=0)
|
||||||
|
{
|
||||||
|
b3SoundMessage msg;
|
||||||
|
msg.m_attackRate = audioSrc->m_attackRate;
|
||||||
|
msg.m_decayRate = audioSrc->m_decayRate;
|
||||||
|
msg.m_sustainLevel = audioSrc->m_sustainLevel;
|
||||||
|
msg.m_releaseRate = audioSrc->m_releaseRate;
|
||||||
|
msg.m_amplitude = audioSrc->m_gain;
|
||||||
|
msg.m_frequency = audioSrc->m_pitch;
|
||||||
|
msg.m_type = B3_SOUND_SOURCE_WAV_FILE;
|
||||||
|
msg.m_wavId = audioSrc->m_userIndex;
|
||||||
|
msg.m_autoKeyOff = true;
|
||||||
|
m_data->m_soundEngine.startSound(soundSourceIndex,msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
int linkIndexA = -1;
|
|
||||||
int linkIndexB = -1;
|
|
||||||
|
|
||||||
int objectIndexB = -1;
|
|
||||||
|
|
||||||
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
|
|
||||||
if (bodyB)
|
|
||||||
{
|
|
||||||
objectIndexB = bodyB->getUserIndex2();
|
|
||||||
}
|
|
||||||
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
|
|
||||||
if (mblB && mblB->m_multiBody)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
#endif//B3_ENABLE_TINY_AUDIO
|
#endif//B3_ENABLE_TINY_AUDIO
|
||||||
}
|
}
|
||||||
@@ -1623,9 +1643,6 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
|||||||
|
|
||||||
#ifdef B3_ENABLE_TINY_AUDIO
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
m_data->m_soundEngine.init(16,true);
|
m_data->m_soundEngine.init(16,true);
|
||||||
m_data->m_wavIds[0] = m_data->m_soundEngine.loadWavFile("wav/cardboardbox0.wav");
|
|
||||||
m_data->m_wavIds[1] = m_data->m_soundEngine.loadWavFile("wav/cardboardbox1.wav");
|
|
||||||
m_data->m_wavIds[2] = m_data->m_soundEngine.loadWavFile("wav/cardboardbox2.wav");
|
|
||||||
|
|
||||||
//we don't use those callbacks (yet), experimental
|
//we don't use those callbacks (yet), experimental
|
||||||
// gContactAddedCallback = MyContactAddedCallback;
|
// gContactAddedCallback = MyContactAddedCallback;
|
||||||
@@ -2039,7 +2056,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
|||||||
|
|
||||||
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
||||||
|
|
||||||
|
|
||||||
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
|
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
|
||||||
|
|
||||||
|
|
||||||
@@ -2111,6 +2127,22 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
|||||||
}
|
}
|
||||||
createJointMotors(mb);
|
createJointMotors(mb);
|
||||||
|
|
||||||
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
|
{
|
||||||
|
SDFAudioSource audioSource;
|
||||||
|
int urdfRootLink = u2b.getRootLinkIndex();//LinkIndex = creation.m_mb2urdfLink[-1];
|
||||||
|
if (u2b.getLinkAudioSource(urdfRootLink,audioSource))
|
||||||
|
{
|
||||||
|
int flags = mb->getBaseCollider()->getCollisionFlags();
|
||||||
|
mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
|
||||||
|
audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
|
||||||
|
if (audioSource.m_userIndex>=0)
|
||||||
|
{
|
||||||
|
bodyHandle->m_audioSources.insert(-1, audioSource);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
|
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
|
||||||
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
|
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
|
||||||
@@ -2130,6 +2162,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
|||||||
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
|
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
|
||||||
for (int i=0;i<mb->getNumLinks();i++)
|
for (int i=0;i<mb->getNumLinks();i++)
|
||||||
{
|
{
|
||||||
|
int link=i;
|
||||||
//disable serialization of the collision objects
|
//disable serialization of the collision objects
|
||||||
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
|
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
|
||||||
int urdfLinkIndex = creation.m_mb2urdfLink[i];
|
int urdfLinkIndex = creation.m_mb2urdfLink[i];
|
||||||
@@ -2148,8 +2181,23 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
|||||||
m_data->m_strings.push_back(jointName);
|
m_data->m_strings.push_back(jointName);
|
||||||
util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str());
|
util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str());
|
||||||
mb->getLink(i).m_jointName = jointName->c_str();
|
mb->getLink(i).m_jointName = jointName->c_str();
|
||||||
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
|
{
|
||||||
|
SDFAudioSource audioSource;
|
||||||
|
int urdfLinkIndex = creation.m_mb2urdfLink[link];
|
||||||
|
if (u2b.getLinkAudioSource(urdfLinkIndex,audioSource))
|
||||||
|
{
|
||||||
|
int flags = mb->getLink(link).m_collider->getCollisionFlags();
|
||||||
|
mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
|
||||||
|
audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
|
||||||
|
if (audioSource.m_userIndex>=0)
|
||||||
|
{
|
||||||
|
bodyHandle->m_audioSources.insert(link, audioSource);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
||||||
m_data->m_strings.push_back(baseName);
|
m_data->m_strings.push_back(baseName);
|
||||||
|
|
||||||
|
|||||||
@@ -23,6 +23,7 @@ b3ADSR::b3ADSR()
|
|||||||
m_releaseRate = 0.0005;
|
m_releaseRate = 0.0005;
|
||||||
m_sustainLevel = 0.5;
|
m_sustainLevel = 0.5;
|
||||||
m_state = ADSR_IDLE;
|
m_state = ADSR_IDLE;
|
||||||
|
m_autoKeyOff = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3ADSR::~b3ADSR()
|
b3ADSR::~b3ADSR()
|
||||||
@@ -51,6 +52,10 @@ double b3ADSR::tick()
|
|||||||
{
|
{
|
||||||
m_value = m_sustainLevel;
|
m_value = m_sustainLevel;
|
||||||
m_state = ADSR_SUSTAIN;
|
m_state = ADSR_SUSTAIN;
|
||||||
|
if (m_autoKeyOff)
|
||||||
|
{
|
||||||
|
keyOff();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@@ -60,6 +65,11 @@ double b3ADSR::tick()
|
|||||||
{
|
{
|
||||||
m_value = m_sustainLevel;
|
m_value = m_sustainLevel;
|
||||||
m_state = ADSR_SUSTAIN;
|
m_state = ADSR_SUSTAIN;
|
||||||
|
if (m_autoKeyOff)
|
||||||
|
{
|
||||||
|
keyOff();
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@@ -81,8 +91,9 @@ bool b3ADSR::isIdle() const
|
|||||||
return m_state == ADSR_IDLE;
|
return m_state == ADSR_IDLE;
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3ADSR::keyOn()
|
void b3ADSR::keyOn(bool autoKeyOff)
|
||||||
{
|
{
|
||||||
|
m_autoKeyOff = autoKeyOff;
|
||||||
if (m_target <= 0.0)
|
if (m_target <= 0.0)
|
||||||
m_target = 1.0;
|
m_target = 1.0;
|
||||||
if (m_attackRate==1)
|
if (m_attackRate==1)
|
||||||
@@ -94,6 +105,7 @@ void b3ADSR::keyOn()
|
|||||||
|
|
||||||
void b3ADSR::keyOff()
|
void b3ADSR::keyOff()
|
||||||
{
|
{
|
||||||
|
m_autoKeyOff = false;
|
||||||
m_target = 0.0;
|
m_target = 0.0;
|
||||||
m_state = ADSR_RELEASE;
|
m_state = ADSR_RELEASE;
|
||||||
|
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ class b3ADSR
|
|||||||
double m_releaseRate;
|
double m_releaseRate;
|
||||||
double m_releaseTime;
|
double m_releaseTime;
|
||||||
double m_sustainLevel;
|
double m_sustainLevel;
|
||||||
|
bool m_autoKeyOff;
|
||||||
public:
|
public:
|
||||||
|
|
||||||
b3ADSR();
|
b3ADSR();
|
||||||
@@ -19,7 +19,7 @@ public:
|
|||||||
|
|
||||||
double tick();
|
double tick();
|
||||||
bool isIdle() const;
|
bool isIdle() const;
|
||||||
void keyOn();
|
void keyOn(bool autoKeyOff);
|
||||||
void keyOff();
|
void keyOff();
|
||||||
|
|
||||||
void setValues(double attack,double decay,double sustain,double release)
|
void setValues(double attack,double decay,double sustain,double release)
|
||||||
|
|||||||
@@ -7,6 +7,8 @@
|
|||||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||||
#include "b3ReadWavFile.h"
|
#include "b3ReadWavFile.h"
|
||||||
#include "../Utils/b3ResourcePath.h"
|
#include "../Utils/b3ResourcePath.h"
|
||||||
|
#include "../Utils/b3HashString.h"
|
||||||
|
|
||||||
#include "Bullet3Common/b3HashMap.h"
|
#include "Bullet3Common/b3HashMap.h"
|
||||||
|
|
||||||
// The default real-time audio input and output buffer size. If
|
// The default real-time audio input and output buffer size. If
|
||||||
@@ -24,6 +26,8 @@ struct b3SoundEngineInternalData
|
|||||||
|
|
||||||
b3AlignedObjectArray<b3SoundSource*> m_soundSources;
|
b3AlignedObjectArray<b3SoundSource*> m_soundSources;
|
||||||
b3HashMap<b3HashInt, b3ReadWavFile*> m_wavFiles;
|
b3HashMap<b3HashInt, b3ReadWavFile*> m_wavFiles;
|
||||||
|
b3HashMap<b3HashString, int> m_name2wav;
|
||||||
|
|
||||||
int m_wavFileUidGenerator;
|
int m_wavFileUidGenerator;
|
||||||
|
|
||||||
b3SoundEngineInternalData()
|
b3SoundEngineInternalData()
|
||||||
@@ -84,6 +88,19 @@ void b3SoundEngine::exit()
|
|||||||
delete m_data->m_soundSources[i];
|
delete m_data->m_soundSources[i];
|
||||||
}
|
}
|
||||||
m_data->m_soundSources.clear();
|
m_data->m_soundSources.clear();
|
||||||
|
|
||||||
|
for (int i=0;i<m_data->m_wavFiles.size();i++)
|
||||||
|
{
|
||||||
|
b3ReadWavFile** wavPtr = m_data->m_wavFiles.getAtIndex(i);
|
||||||
|
if (wavPtr && *wavPtr)
|
||||||
|
{
|
||||||
|
b3ReadWavFile* wav = *wavPtr;
|
||||||
|
delete wav;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
m_data->m_wavFiles.clear();
|
||||||
|
m_data->m_name2wav.clear();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int b3SoundEngine::getAvailableSoundSource()
|
int b3SoundEngine::getAvailableSoundSource()
|
||||||
@@ -113,7 +130,7 @@ void b3SoundEngine::startSound(int soundSourceIndex, b3SoundMessage msg)
|
|||||||
soundSource->setOscillatorFrequency(0, msg.m_frequency);
|
soundSource->setOscillatorFrequency(0, msg.m_frequency);
|
||||||
soundSource->setOscillatorFrequency(1, msg.m_frequency);
|
soundSource->setOscillatorFrequency(1, msg.m_frequency);
|
||||||
|
|
||||||
soundSource->startSound();
|
soundSource->startSound(msg.m_autoKeyOff);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case B3_SOUND_SOURCE_WAV_FILE:
|
case B3_SOUND_SOURCE_WAV_FILE:
|
||||||
@@ -124,7 +141,7 @@ void b3SoundEngine::startSound(int soundSourceIndex, b3SoundMessage msg)
|
|||||||
b3ReadWavFile* wavFile = *wavFilePtr;
|
b3ReadWavFile* wavFile = *wavFilePtr;
|
||||||
soundSource->setWavFile(0,wavFile,getSampleRate());
|
soundSource->setWavFile(0,wavFile,getSampleRate());
|
||||||
soundSource->setWavFile(1,wavFile,getSampleRate());
|
soundSource->setWavFile(1,wavFile,getSampleRate());
|
||||||
soundSource->startSound();
|
soundSource->startSound(msg.m_autoKeyOff);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -142,6 +159,11 @@ void b3SoundEngine::releaseSound(int soundSourceIndex)
|
|||||||
|
|
||||||
int b3SoundEngine::loadWavFile(const char* fileName)
|
int b3SoundEngine::loadWavFile(const char* fileName)
|
||||||
{
|
{
|
||||||
|
int* wavUidPtr = m_data->m_name2wav[fileName];
|
||||||
|
if (wavUidPtr)
|
||||||
|
{
|
||||||
|
return *wavUidPtr;
|
||||||
|
}
|
||||||
char resourcePath[1024];
|
char resourcePath[1024];
|
||||||
|
|
||||||
if (b3ResourcePath::findResourcePath(fileName, resourcePath, 1024))
|
if (b3ResourcePath::findResourcePath(fileName, resourcePath, 1024))
|
||||||
@@ -153,6 +175,7 @@ int b3SoundEngine::loadWavFile(const char* fileName)
|
|||||||
wavFile->normalize(1);
|
wavFile->normalize(1);
|
||||||
int wavUID = m_data->m_wavFileUidGenerator++;
|
int wavUID = m_data->m_wavFileUidGenerator++;
|
||||||
m_data->m_wavFiles.insert(wavUID, wavFile);
|
m_data->m_wavFiles.insert(wavUID, wavFile);
|
||||||
|
m_data->m_name2wav.insert(fileName,wavUID);
|
||||||
return wavUID;
|
return wavUID;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
@@ -16,6 +16,7 @@ struct b3SoundMessage
|
|||||||
double m_decayRate;
|
double m_decayRate;
|
||||||
double m_sustainLevel;
|
double m_sustainLevel;
|
||||||
double m_releaseRate;
|
double m_releaseRate;
|
||||||
|
bool m_autoKeyOff;
|
||||||
|
|
||||||
b3SoundMessage()
|
b3SoundMessage()
|
||||||
:m_type(B3_SOUND_SOURCE_SINE_OSCILLATOR),
|
:m_type(B3_SOUND_SOURCE_SINE_OSCILLATOR),
|
||||||
@@ -25,7 +26,8 @@ struct b3SoundMessage
|
|||||||
m_attackRate(0.001),
|
m_attackRate(0.001),
|
||||||
m_decayRate(0.00001),
|
m_decayRate(0.00001),
|
||||||
m_sustainLevel(0.5),
|
m_sustainLevel(0.5),
|
||||||
m_releaseRate(0.0005)
|
m_releaseRate(0.0005),
|
||||||
|
m_autoKeyOff(false)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -183,7 +183,7 @@ bool b3SoundSource::isAvailable() const
|
|||||||
return m_data->m_envelope.isIdle();
|
return m_data->m_envelope.isIdle();
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3SoundSource::startSound()
|
void b3SoundSource::startSound(bool autoKeyOff)
|
||||||
{
|
{
|
||||||
if (m_data->m_envelope.isIdle())
|
if (m_data->m_envelope.isIdle())
|
||||||
{
|
{
|
||||||
@@ -209,7 +209,7 @@ void b3SoundSource::startSound()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
m_data->m_envelope.keyOn();
|
m_data->m_envelope.keyOn(autoKeyOff);
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3SoundSource::stopSound()
|
void b3SoundSource::stopSound()
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ public:
|
|||||||
|
|
||||||
bool setWavFile(int oscillatorIndex, class b3ReadWavFile* wavFilePtr, int sampleRate);
|
bool setWavFile(int oscillatorIndex, class b3ReadWavFile* wavFilePtr, int sampleRate);
|
||||||
|
|
||||||
void startSound();
|
void startSound(bool autoKeyOff);
|
||||||
void stopSound();
|
void stopSound();
|
||||||
|
|
||||||
bool isAvailable() const;
|
bool isAvailable() const;
|
||||||
|
|||||||
59
examples/Utils/b3HashString.h
Normal file
59
examples/Utils/b3HashString.h
Normal file
@@ -0,0 +1,59 @@
|
|||||||
|
#ifndef B3_HASH_STRING_H
|
||||||
|
#define B3_HASH_STRING_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;
|
||||||
|
|
||||||
|
for(int i = 0; m_string[i]; 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif //B3_HASH_STRING_H
|
||||||
@@ -19,58 +19,6 @@ subject to the following restrictions:
|
|||||||
|
|
||||||
#include "b3AlignedObjectArray.h"
|
#include "b3AlignedObjectArray.h"
|
||||||
|
|
||||||
///very basic hashable string implementation, compatible with b3HashMap
|
|
||||||
struct b3HashString
|
|
||||||
{
|
|
||||||
const char* 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;
|
|
||||||
|
|
||||||
for(int i = 0; m_string[i]; 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) ||
|
|
||||||
(0==portableStringCompare(m_string,other.m_string));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
const int B3_HASH_NULL=0xffffffff;
|
const int B3_HASH_NULL=0xffffffff;
|
||||||
|
|
||||||
|
|||||||
@@ -140,6 +140,7 @@ public:
|
|||||||
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
|
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
|
||||||
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
|
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
|
||||||
CF_HAS_FRICTION_ANCHOR = 512,
|
CF_HAS_FRICTION_ANCHOR = 512,
|
||||||
|
CF_HAS_COLLISION_SOUND_TRIGGER = 1024
|
||||||
};
|
};
|
||||||
|
|
||||||
enum CollisionObjectTypes
|
enum CollisionObjectTypes
|
||||||
|
|||||||
@@ -79,6 +79,11 @@ class btHashInt
|
|||||||
{
|
{
|
||||||
int m_uid;
|
int m_uid;
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
btHashInt()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
btHashInt(int uid) :m_uid(uid)
|
btHashInt(int uid) :m_uid(uid)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user