This commit is contained in:
Erwin Coumans (google)
2017-05-12 20:53:24 +00:00
179 changed files with 21517 additions and 1172 deletions

View File

@@ -158,6 +158,12 @@ end
description = "Double precision version of Bullet"
}
newoption
{
trigger = "serial",
description = "Enable serial, for testing the VR glove in C++"
}
newoption
{
trigger = "audio",
@@ -259,6 +265,10 @@ end
include "../examples/TinyAudio"
end
if _OPTIONS["serial"] then
include "../examples/ThirdPartyLibs/serial"
end
if not _OPTIONS["no-demos"] then
include "../examples/ExampleBrowser"
include "../examples/RobotSimulator"

View File

@@ -17,6 +17,9 @@ del tmp1234.txt
cd build3
premake4 --double --midi --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
premake4 --double --midi --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
#premake4 --serial --audio --double --midi --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
start vs2010

View File

@@ -25,7 +25,7 @@
<default>
<default class="MPL">
<geom material="MatMesh" contype="1" conaffinity="1" condim="4" margin="0.001"/>
<geom material="MatMesh" contype="1" conaffinity="1" condim="4" friction="1 .5 0.5" margin="0.001"/>
<joint limited="true" damping="0.2" armature=".01"/>
<site material="MatTouch" type="ellipsoid" group="3"/>
<position ctrllimited="true" kp="10"/>
@@ -104,7 +104,7 @@
<!-- ======= ROBOT ======= -->
<body name="wristy" pos="0 0 0">
<body childclass="MPL" name="wristy" pos="0 0 0">
<inertial pos="-7.08369e-005 -0.0217787 -0.000286168" quat="0.707488 0.00581744 -0.0107421 0.70662" mass="0.0272932" diaginertia="2.46813e-005 1.77029e-005 1.71079e-005" />
<geom type="mesh" mesh="wristy"/>
<joint type="free"/>

BIN
data/example_log_vr.bin Executable file

Binary file not shown.

View File

@@ -0,0 +1,118 @@
<mujoco model="humanoid">
<compiler angle="degree" inertiafromgeom="true"/>
<default>
<joint armature="1" damping="1" limited="true"/>
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1"/>
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
</default>
<option integrator="RK4" iterations="50" solver="PGS" timestep="0.003">
<!-- <flags solverstat="enable" energy="enable"/>-->
</option>
<size nkey="5" nuser_geom="1"/>
<visual>
<map fogend="5" fogstart="3"/>
</visual>
<asset>
<texture builtin="gradient" height="100" rgb1=".4 .5 .6" rgb2="0 0 0" type="skybox" width="100"/>
<!-- <texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>-->
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<!-- light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/-->
<geom condim="3" friction="1 .1 .1" material="MatPlane" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 0.125" type="plane"/>
<!-- <geom condim="3" material="MatPlane" name="floor" pos="0 0 0" size="10 10 0.125" type="plane"/>-->
<body name="torso" pos="0 0 1.4">
<!--joint armature="0" damping="0" limited="false" name="root" pos="0 0 0" stiffness="0" type="free"/-->
<geom fromto="0 -.07 0 0 .07 0" name="torso1" size="0.07" type="capsule"/>
<geom name="head" pos="0 0 .19" size=".09" type="sphere" user="258"/>
<geom fromto="-.01 -.06 -.12 -.01 .06 -.12" name="uwaist" size="0.06" type="capsule"/>
<body name="lwaist" pos="-.01 0 -0.260" quat="1.000 0 -0.002 0">
<geom fromto="0 -.06 0 0 .06 0" name="lwaist" size="0.06" type="capsule"/>
<joint armature="0.02" axis="0 0 1" damping="5" name="abdomen_z" pos="0 0 0.065" range="-45 45" stiffness="20" type="hinge"/>
<joint armature="0.02" axis="0 1 0" damping="5" name="abdomen_y" pos="0 0 0.065" range="-75 30" stiffness="10" type="hinge"/>
<body name="pelvis" pos="0 0 -0.165" quat="1.000 0 -0.002 0">
<joint armature="0.02" axis="1 0 0" damping="5" name="abdomen_x" pos="0 0 0.1" range="-35 35" stiffness="10" type="hinge"/>
<geom fromto="-.02 -.07 0 -.02 .07 0" name="butt" size="0.09" type="capsule"/>
<body name="right_thigh" pos="0 -0.1 -0.04">
<joint armature="0.01" axis="1 0 0" damping="5" name="right_hip_x" pos="0 0 0" range="-25 5" stiffness="10" type="hinge"/>
<joint armature="0.01" axis="0 0 1" damping="5" name="right_hip_z" pos="0 0 0" range="-60 35" stiffness="10" type="hinge"/>
<joint armature="0.01" axis="0 1 0" damping="5" name="right_hip_y" pos="0 0 0" range="-120 20" stiffness="20" type="hinge"/>
<geom fromto="0 0 0 0 0.01 -.34" name="right_thigh1" size="0.06" type="capsule"/>
<body name="right_shin" pos="0 0.01 -0.403">
<joint armature="0.0060" axis="0 -1 0" name="right_knee" pos="0 0 .02" range="-160 -2" stiffness="1" type="hinge"/>
<geom fromto="0 0 0 0 0 -.3" name="right_shin1" size="0.049" type="capsule"/>
<body name="right_foot" pos="0 0 -0.45">
<geom name="right_foot" pos="0 0 0.1" size="0.075" type="sphere" user="0"/>
</body>
</body>
</body>
<body name="left_thigh" pos="0 0.1 -0.04">
<joint armature="0.01" axis="-1 0 0" damping="5" name="left_hip_x" pos="0 0 0" range="-25 5" stiffness="10" type="hinge"/>
<joint armature="0.01" axis="0 0 -1" damping="5" name="left_hip_z" pos="0 0 0" range="-60 35" stiffness="10" type="hinge"/>
<joint armature="0.01" axis="0 1 0" damping="5" name="left_hip_y" pos="0 0 0" range="-120 20" stiffness="20" type="hinge"/>
<geom fromto="0 0 0 0 -0.01 -.34" name="left_thigh1" size="0.06" type="capsule"/>
<body name="left_shin" pos="0 -0.01 -0.403">
<joint armature="0.0060" axis="0 -1 0" name="left_knee" pos="0 0 .02" range="-160 -2" stiffness="1" type="hinge"/>
<geom fromto="0 0 0 0 0 -.3" name="left_shin1" size="0.049" type="capsule"/>
<body name="left_foot" pos="0 0 -0.45">
<geom name="left_foot" type="sphere" size="0.075" pos="0 0 0.1" user="0" />
</body>
</body>
</body>
</body>
</body>
<body name="right_upper_arm" pos="0 -0.17 0.06">
<joint armature="0.0068" axis="2 1 1" name="right_shoulder1" pos="0 0 0" range="-85 60" stiffness="1" type="hinge"/>
<joint armature="0.0051" axis="0 -1 1" name="right_shoulder2" pos="0 0 0" range="-85 60" stiffness="1" type="hinge"/>
<geom fromto="0 0 0 .16 -.16 -.16" name="right_uarm1" size="0.04 0.16" type="capsule"/>
<body name="right_lower_arm" pos=".18 -.18 -.18">
<joint armature="0.0028" axis="0 -1 1" name="right_elbow" pos="0 0 0" range="-90 50" stiffness="0" type="hinge"/>
<geom fromto="0.01 0.01 0.01 .17 .17 .17" name="right_larm" size="0.031" type="capsule"/>
<geom name="right_hand" pos=".18 .18 .18" size="0.04" type="sphere"/>
</body>
</body>
<body name="left_upper_arm" pos="0 0.17 0.06">
<joint armature="0.0068" axis="2 -1 1" name="left_shoulder1" pos="0 0 0" range="-60 85" stiffness="1" type="hinge"/>
<joint armature="0.0051" axis="0 1 1" name="left_shoulder2" pos="0 0 0" range="-60 85" stiffness="1" type="hinge"/>
<geom fromto="0 0 0 .16 .16 -.16" name="left_uarm1" size="0.04 0.16" type="capsule"/>
<body name="left_lower_arm" pos=".18 .18 -.18">
<joint armature="0.0028" axis="0 -1 -1" name="left_elbow" pos="0 0 0" range="-90 50" stiffness="0" type="hinge"/>
<geom fromto="0.01 -0.01 0.01 .17 -.17 .17" name="left_larm" size="0.031" type="capsule"/>
<geom name="left_hand" pos=".18 -.18 .18" size="0.04" type="sphere"/>
</body>
</body>
</body>
</worldbody>
<tendon>
<fixed name="left_hipknee">
<joint coef="-1" joint="left_hip_y"/>
<joint coef="1" joint="left_knee"/>
</fixed>
<fixed name="right_hipknee">
<joint coef="-1" joint="right_hip_y"/>
<joint coef="1" joint="right_knee"/>
</fixed>
</tendon>
<actuator><!-- this section is not supported, same constants in code -->
<motor gear="100" joint="abdomen_y" name="abdomen_y"/>
<motor gear="100" joint="abdomen_z" name="abdomen_z"/>
<motor gear="100" joint="abdomen_x" name="abdomen_x"/>
<motor gear="100" joint="right_hip_x" name="right_hip_x"/>
<motor gear="100" joint="right_hip_z" name="right_hip_z"/>
<motor gear="300" joint="right_hip_y" name="right_hip_y"/>
<motor gear="200" joint="right_knee" name="right_knee"/>
<motor gear="100" joint="left_hip_x" name="left_hip_x"/>
<motor gear="100" joint="left_hip_z" name="left_hip_z"/>
<motor gear="300" joint="left_hip_y" name="left_hip_y"/>
<motor gear="200" joint="left_knee" name="left_knee"/>
<motor gear="25" joint="right_shoulder1" name="right_shoulder1"/>
<motor gear="25" joint="right_shoulder2" name="right_shoulder2"/>
<motor gear="25" joint="right_elbow" name="right_elbow"/>
<motor gear="25" joint="left_shoulder1" name="left_shoulder1"/>
<motor gear="25" joint="left_shoulder2" name="left_shoulder2"/>
<motor gear="25" joint="left_elbow" name="left_elbow"/>
</actuator>
</mujoco>

Binary file not shown.

View 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.04</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>

BIN
data/wav/cardboardbox0.wav Normal file

Binary file not shown.

BIN
data/wav/cardboardbox1.wav Normal file

Binary file not shown.

BIN
data/wav/cardboardbox2.wav Normal file

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@@ -187,6 +187,7 @@ files {
"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
"../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
"../ThirdPartyLibs/openvr/samples/shared/strtools.cpp",
"../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
"../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
"../ThirdPartyLibs/openvr/samples/shared/Vectors.h",

View File

@@ -37,7 +37,8 @@ struct GUIHelperInterface
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) = 0;
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0;
virtual void removeAllGraphicsInstances()=0;
virtual void removeGraphicsInstance(int graphicsUid) {}
virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
virtual CommonParameterInterface* getParameterInterface()=0;
@@ -123,7 +124,8 @@ struct DummyGUIHelper : public GUIHelperInterface
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId){return -1;}
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;}
virtual void removeAllGraphicsInstances(){}
virtual void removeGraphicsInstance(int graphicsUid){}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;

View File

@@ -26,7 +26,7 @@ struct SliderParams
m_callback(0),
m_paramValuePointer(targetValuePointer),
m_userPointer(0),
m_clampToNotches(true),
m_clampToNotches(false),
m_clampToIntegers(false),
m_showValues(true)
{

View File

@@ -23,11 +23,14 @@ struct CommonRenderInterface
virtual void init()=0;
virtual void updateCamera(int upAxis)=0;
virtual void removeAllInstances() = 0;
virtual void removeGraphicsInstance(int instanceUid) = 0;
virtual const CommonCameraInterface* getActiveCamera() const =0;
virtual CommonCameraInterface* getActiveCamera()=0;
virtual void setActiveCamera(CommonCameraInterface* cam)=0;
virtual void setLightPosition(const float lightPos[3]) = 0;
virtual void setLightPosition(const double lightPos[3]) = 0;
virtual void renderScene()=0;
virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};

View File

@@ -410,6 +410,18 @@ struct CommonRigidBodyBase : public CommonExampleInterface
return box;
}
void deleteRigidBody(btRigidBody* body)
{
int graphicsUid = body->getUserIndex();
m_guiHelper->removeGraphicsInstance(graphicsUid);
m_dynamicsWorld->removeRigidBody(body);
btMotionState* ms = body->getMotionState();
delete body;
delete ms;
}
btRigidBody* createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape, const btVector4& color = btVector4(1, 0, 0, 1))
{
btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));

View File

@@ -52,6 +52,10 @@
#include "../RoboticsLearning/GripperGraspExample.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
#ifdef B3_ENABLE_TINY_AUDIO
#include "../TinyAudio/TinyAudioExample.h"
#endif //B3_ENABLE_TINY_AUDIO
#ifdef ENABLE_LUA
#include "../LuaDemo/LuaPhysicsSetup.h"
#endif
@@ -100,7 +104,6 @@ struct ExampleEntry
static ExampleEntry gDefaultExamples[]=
{
ExampleEntry(0,"API"),
ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
@@ -307,7 +310,11 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"TinyRenderer", "Very small software renderer.", TinyRendererCreateFunc),
ExampleEntry(1,"Dynamic Texture", "Dynamic updated textured applied to a cube.", DynamicTexturedCubeDemoCreateFunc),
#ifdef B3_ENABLE_TINY_AUDIO
ExampleEntry(0,"Audio"),
ExampleEntry(1,"Simple Audio","Play some sound", TinyAudioExampleCreateFunc),
#endif
//Extended Tutorials Added by Mobeen
ExampleEntry(0,"Extended Tutorials"),

View File

@@ -48,7 +48,6 @@
//quick test for file import, @todo(erwincoumans) make it more general and add other file formats
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportBullet/SerializeSetup.h"
#include "Bullet3Common/b3HashMap.h"
struct GL3TexLoader : public MyTextureLoader
@@ -261,7 +260,7 @@ void MyKeyboardCallback(int key, int state)
} else
{
b3ChromeUtilsStopTimingsAndWriteJsonFile();
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
}
#endif //BT_NO_PROFILE
}

View File

@@ -7,6 +7,7 @@
#include "Bullet3Common/b3Scalar.h"
#include "CollisionShape2TriangleMesh.h"
#include "../OpenGLWindow/ShapeData.h"
#include "../OpenGLWindow/SimpleCamera.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
@@ -35,6 +36,8 @@ ATTRIBUTE_ALIGNED16( class )MyDebugDrawer : public btIDebugDraw
btAlignedObjectArray<MyDebugVec3> m_linePoints;
btAlignedObjectArray<unsigned int> m_lineIndices;
btVector3 m_currentLineColor;
DefaultColors m_ourColors;
@@ -133,14 +136,67 @@ public:
static btVector4 sColors[4] =
{
btVector4(0.3,0.3,1,1),
btVector4(0.6,0.6,1,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(60./256.,186./256.,84./256.,1),
btVector4(244./256.,194./256.,13./256.,1),
btVector4(219./256.,50./256.,54./256.,1),
btVector4(72./256.,133./256.,237./256.,1),
//btVector4(1,1,0,1),
};
struct MyHashShape
{
int m_shapeKey;
int m_shapeType;
btVector3 m_sphere0Pos;
btVector3 m_sphere1Pos;
btScalar m_radius0;
btScalar m_radius1;
btTransform m_childTransform;
int m_deformFunc;
int m_upAxis;
btScalar m_halfHeight;
MyHashShape()
:m_shapeKey(0),
m_shapeType(0),
m_sphere0Pos(btVector3(0,0,0)),
m_sphere1Pos(btVector3(0,0,0)),
m_radius0(0),
m_radius1(0),
m_deformFunc(0),
m_upAxis(-1),
m_halfHeight(0)
{
m_childTransform.setIdentity();
}
bool equals(const MyHashShape& other) const
{
bool sameShapeType = m_shapeType==other.m_shapeType;
bool sameSphere0= m_sphere0Pos == other.m_sphere0Pos;
bool sameSphere1= m_sphere1Pos == other.m_sphere1Pos;
bool sameRadius0 = m_radius0== other.m_radius0;
bool sameRadius1 = m_radius1== other.m_radius1;
bool sameTransform = m_childTransform== other.m_childTransform;
bool sameUpAxis = m_upAxis == other.m_upAxis;
bool sameHalfHeight = m_halfHeight == other.m_halfHeight;
return sameShapeType && sameSphere0 && sameSphere1 && sameRadius0 && sameRadius1 && sameTransform && sameUpAxis && sameHalfHeight;
}
//to our success
SIMD_FORCE_INLINE unsigned int getHash()const
{
unsigned int key = m_shapeKey;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
return key;
}
};
struct OpenGLGuiHelperInternalData
{
struct CommonGraphicsApp* m_glApp;
@@ -150,12 +206,20 @@ struct OpenGLGuiHelperInternalData
btAlignedObjectArray<unsigned char> m_rgbaPixelBuffer1;
btAlignedObjectArray<float> m_depthBuffer1;
btHashMap<MyHashShape, int> m_hashShapes;
VisualizerFlagCallback m_visualizerFlagCallback;
int m_checkedTexture;
int m_checkedTextureGrey;
OpenGLGuiHelperInternalData()
:m_vrMode(false),
m_vrSkipShadowPass(0),
m_visualizerFlagCallback(0)
m_visualizerFlagCallback(0),
m_checkedTexture(-1),
m_checkedTextureGrey(-1)
{
}
@@ -238,19 +302,530 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit
void OpenGLGuiHelper::removeAllGraphicsInstances()
{
m_data->m_hashShapes.clear();
m_data->m_glApp->m_renderer->removeAllInstances();
}
void OpenGLGuiHelper::removeGraphicsInstance(int graphicsUid)
{
if (graphicsUid>=0)
{
m_data->m_glApp->m_renderer->removeGraphicsInstance(graphicsUid);
};
}
int OpenGLGuiHelper::createCheckeredTexture(int red,int green, int blue)
{
int texWidth=1024;
int texHeight=1024;
btAlignedObjectArray<unsigned char> texels;
texels.resize(texWidth*texHeight*3);
for (int i=0;i<texWidth*texHeight*3;i++)
texels[i]=255;
for (int i=0;i<texWidth;i++)
{
for (int j=0;j<texHeight;j++)
{
int a = i<texWidth/2? 1 : 0;
int b = j<texWidth/2? 1 : 0;
if (a==b)
{
texels[(i+j*texWidth)*3+0] = red;
texels[(i+j*texWidth)*3+1] = green;
texels[(i+j*texWidth)*3+2] = blue;
// texels[(i+j*texWidth)*4+3] = 255;
}
/*else
{
texels[i*3+0+j*texWidth] = 255;
texels[i*3+1+j*texWidth] = 255;
texels[i*3+2+j*texWidth] = 255;
}
*/
}
}
int texId = registerTexture(&texels[0],texWidth,texHeight);
return texId;
}
void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
{
//already has a graphics object?
if (collisionShape->getUserIndex()>=0)
return;
btAlignedObjectArray<GLInstanceVertex> gfxVertices;
if (m_data->m_checkedTexture<0)
{
m_data->m_checkedTexture = createCheckeredTexture(192,192,255);
}
if (m_data->m_checkedTextureGrey<0)
{
m_data->m_checkedTextureGrey = createCheckeredTexture(192,192,192);
}
btAlignedObjectArray<GLInstanceVertex> gfxVertices;
btAlignedObjectArray<int> indices;
int strideInBytes = 9*sizeof(float);
//if (collisionShape->getShapeType()==BOX_SHAPE_PROXYTYPE)
{
}
if (collisionShape->getShapeType()==MULTI_SPHERE_SHAPE_PROXYTYPE)
{
btMultiSphereShape* ms = (btMultiSphereShape*) collisionShape;
if (ms->getSphereCount()==2)
{
btAlignedObjectArray<float> transformedVertices;
int numVertices = sizeof(textured_detailed_sphere_vertices)/strideInBytes;
transformedVertices.resize(numVertices*9);
btVector3 sphere0Pos = ms->getSpherePosition(0);
btVector3 sphere1Pos = ms->getSpherePosition(1);
btVector3 fromTo = sphere1Pos-sphere0Pos;
MyHashShape shape;
shape.m_sphere0Pos = sphere0Pos;
shape.m_sphere1Pos = sphere1Pos;
shape.m_radius0 = 2.*ms->getSphereRadius(0);
shape.m_radius1 = 2.*ms->getSphereRadius(1);
shape.m_deformFunc = 1;//vert.dot(fromTo)
int graphicsShapeIndex = -1;
int* graphicsShapeIndexPtr = m_data->m_hashShapes[shape];
if (graphicsShapeIndexPtr)
{
//cache hit
graphicsShapeIndex = *graphicsShapeIndexPtr;
} else
{
//cache miss
for (int i=0;i<numVertices;i++)
{
btVector3 vert;
vert.setValue(textured_detailed_sphere_vertices[i*9+0],
textured_detailed_sphere_vertices[i*9+1],
textured_detailed_sphere_vertices[i*9+2]);
btVector3 trVer(0,0,0);
if (vert.dot(fromTo)>0)
{
btScalar radiusScale = 2.*ms->getSphereRadius(1);
trVer = radiusScale*vert;
trVer+=sphere1Pos;
} else
{
btScalar radiusScale = 2.*ms->getSphereRadius(0);
trVer = radiusScale*vert;
trVer+=sphere0Pos;
}
transformedVertices[i*9+0] = trVer[0];
transformedVertices[i*9+1] = trVer[1];
transformedVertices[i*9+2] = trVer[2];
transformedVertices[i*9+3] =textured_detailed_sphere_vertices[i*9+3];
transformedVertices[i*9+4] =textured_detailed_sphere_vertices[i*9+4];
transformedVertices[i*9+5] =textured_detailed_sphere_vertices[i*9+5];
transformedVertices[i*9+6] =textured_detailed_sphere_vertices[i*9+6];
transformedVertices[i*9+7] =textured_detailed_sphere_vertices[i*9+7];
transformedVertices[i*9+8] =textured_detailed_sphere_vertices[i*9+8];
}
int numIndices = sizeof(textured_detailed_sphere_indices)/sizeof(int);
graphicsShapeIndex = registerGraphicsShape(&transformedVertices[0],numVertices,textured_detailed_sphere_indices,numIndices,B3_GL_TRIANGLES,m_data->m_checkedTextureGrey);
m_data->m_hashShapes.insert(shape,graphicsShapeIndex);
}
collisionShape->setUserIndex(graphicsShapeIndex);
return;
}
}
if (collisionShape->getShapeType()==SPHERE_SHAPE_PROXYTYPE)
{
btSphereShape* sphereShape = (btSphereShape*) collisionShape;
btScalar radius = sphereShape->getRadius();
btScalar sphereSize = 2.*radius;
btVector3 radiusScale(sphereSize,sphereSize,sphereSize);
btAlignedObjectArray<float> transformedVertices;
MyHashShape shape;
shape.m_radius0 = sphereSize;
shape.m_deformFunc = 0;////no deform
int graphicsShapeIndex = -1;
int* graphicsShapeIndexPtr = m_data->m_hashShapes[shape];
if (graphicsShapeIndexPtr)
{
graphicsShapeIndex = *graphicsShapeIndexPtr;
} else
{
int numVertices = sizeof(textured_detailed_sphere_vertices)/strideInBytes;
transformedVertices.resize(numVertices*9);
for (int i=0;i<numVertices;i++)
{
btVector3 vert;
vert.setValue(textured_detailed_sphere_vertices[i*9+0],
textured_detailed_sphere_vertices[i*9+1],
textured_detailed_sphere_vertices[i*9+2]);
btVector3 trVer = radiusScale*vert;
transformedVertices[i*9+0] = trVer[0];
transformedVertices[i*9+1] = trVer[1];
transformedVertices[i*9+2] = trVer[2];
transformedVertices[i*9+3] =textured_detailed_sphere_vertices[i*9+3];
transformedVertices[i*9+4] =textured_detailed_sphere_vertices[i*9+4];
transformedVertices[i*9+5] =textured_detailed_sphere_vertices[i*9+5];
transformedVertices[i*9+6] =textured_detailed_sphere_vertices[i*9+6];
transformedVertices[i*9+7] =textured_detailed_sphere_vertices[i*9+7];
transformedVertices[i*9+8] =textured_detailed_sphere_vertices[i*9+8];
}
int numIndices = sizeof(textured_detailed_sphere_indices)/sizeof(int);
graphicsShapeIndex = registerGraphicsShape(&transformedVertices[0],numVertices,textured_detailed_sphere_indices,numIndices,B3_GL_TRIANGLES,m_data->m_checkedTextureGrey);
m_data->m_hashShapes.insert(shape,graphicsShapeIndex);
}
collisionShape->setUserIndex(graphicsShapeIndex);
return;
}
if (collisionShape->getShapeType()==COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)collisionShape;
if (compound->getNumChildShapes()==1)
{
if (compound->getChildShape(0)->getShapeType()==SPHERE_SHAPE_PROXYTYPE)
{
btSphereShape* sphereShape = (btSphereShape*) compound->getChildShape(0);
btScalar radius = sphereShape->getRadius();
btScalar sphereSize = 2.*radius;
btVector3 radiusScale(sphereSize,sphereSize,sphereSize);
MyHashShape shape;
shape.m_radius0 = sphereSize;
shape.m_deformFunc = 0;//no deform
shape.m_childTransform = compound->getChildTransform(0);
int graphicsShapeIndex = -1;
int* graphicsShapeIndexPtr = m_data->m_hashShapes[shape];
if (graphicsShapeIndexPtr)
{
graphicsShapeIndex = *graphicsShapeIndexPtr;
}
else
{
btAlignedObjectArray<float> transformedVertices;
int numVertices = sizeof(textured_detailed_sphere_vertices)/strideInBytes;
transformedVertices.resize(numVertices*9);
for (int i=0;i<numVertices;i++)
{
btVector3 vert;
vert.setValue(textured_detailed_sphere_vertices[i*9+0],
textured_detailed_sphere_vertices[i*9+1],
textured_detailed_sphere_vertices[i*9+2]);
btVector3 trVer = compound->getChildTransform(0)*(radiusScale*vert);
transformedVertices[i*9+0] = trVer[0];
transformedVertices[i*9+1] = trVer[1];
transformedVertices[i*9+2] = trVer[2];
transformedVertices[i*9+3] =textured_detailed_sphere_vertices[i*9+3];
transformedVertices[i*9+4] =textured_detailed_sphere_vertices[i*9+4];
transformedVertices[i*9+5] =textured_detailed_sphere_vertices[i*9+5];
transformedVertices[i*9+6] =textured_detailed_sphere_vertices[i*9+6];
transformedVertices[i*9+7] =textured_detailed_sphere_vertices[i*9+7];
transformedVertices[i*9+8] =textured_detailed_sphere_vertices[i*9+8];
}
int numIndices = sizeof(textured_detailed_sphere_indices)/sizeof(int);
graphicsShapeIndex = registerGraphicsShape(&transformedVertices[0],numVertices,textured_detailed_sphere_indices,numIndices,B3_GL_TRIANGLES,m_data->m_checkedTextureGrey);
m_data->m_hashShapes.insert(shape,graphicsShapeIndex);
}
collisionShape->setUserIndex(graphicsShapeIndex);
return;
}
if (compound->getChildShape(0)->getShapeType()==CAPSULE_SHAPE_PROXYTYPE)
{
btCapsuleShape* sphereShape = (btCapsuleShape*) compound->getChildShape(0);
int up = sphereShape->getUpAxis();
btScalar halfHeight = sphereShape->getHalfHeight();
btScalar radius = sphereShape->getRadius();
btScalar sphereSize = 2.*radius;
btVector3 radiusScale = btVector3(sphereSize,sphereSize,sphereSize);
MyHashShape shape;
shape.m_radius0 = sphereSize;
shape.m_deformFunc = 2;//no deform
shape.m_childTransform = compound->getChildTransform(0);
shape.m_upAxis = up;
int graphicsShapeIndex = -1;
int* graphicsShapeIndexPtr = m_data->m_hashShapes[shape];
if (graphicsShapeIndexPtr)
{
graphicsShapeIndex = *graphicsShapeIndexPtr;
}
else
{
btAlignedObjectArray<float> transformedVertices;
int numVertices = sizeof(textured_detailed_sphere_vertices)/strideInBytes;
transformedVertices.resize(numVertices*9);
for (int i=0;i<numVertices;i++)
{
btVector3 vert;
vert.setValue(textured_detailed_sphere_vertices[i*9+0],
textured_detailed_sphere_vertices[i*9+1],
textured_detailed_sphere_vertices[i*9+2]);
btVector3 trVer = compound->getChildTransform(0)*(radiusScale*vert);
if (trVer[up]>0)
trVer[up]+=halfHeight;
else
trVer[up]-=halfHeight;
transformedVertices[i*9+0] = trVer[0];
transformedVertices[i*9+1] = trVer[1];
transformedVertices[i*9+2] = trVer[2];
transformedVertices[i*9+3] =textured_detailed_sphere_vertices[i*9+3];
transformedVertices[i*9+4] =textured_detailed_sphere_vertices[i*9+4];
transformedVertices[i*9+5] =textured_detailed_sphere_vertices[i*9+5];
transformedVertices[i*9+6] =textured_detailed_sphere_vertices[i*9+6];
transformedVertices[i*9+7] =textured_detailed_sphere_vertices[i*9+7];
transformedVertices[i*9+8] =textured_detailed_sphere_vertices[i*9+8];
}
int numIndices = sizeof(textured_detailed_sphere_indices)/sizeof(int);
graphicsShapeIndex = registerGraphicsShape(&transformedVertices[0],numVertices,textured_detailed_sphere_indices,numIndices,B3_GL_TRIANGLES,m_data->m_checkedTextureGrey);
m_data->m_hashShapes.insert(shape,graphicsShapeIndex);
}
collisionShape->setUserIndex(graphicsShapeIndex);
return;
}
if (compound->getChildShape(0)->getShapeType()==MULTI_SPHERE_SHAPE_PROXYTYPE)
{
btMultiSphereShape* ms = (btMultiSphereShape*) compound->getChildShape(0);
if (ms->getSphereCount()==2)
{
btAlignedObjectArray<float> transformedVertices;
int numVertices = sizeof(textured_detailed_sphere_vertices)/strideInBytes;
transformedVertices.resize(numVertices*9);
btVector3 sphere0Pos = ms->getSpherePosition(0);
btVector3 sphere1Pos = ms->getSpherePosition(1);
btVector3 fromTo = sphere1Pos-sphere0Pos;
btScalar radiusScale1 = 2.0*ms->getSphereRadius(1);
btScalar radiusScale0 = 2.0*ms->getSphereRadius(0);
MyHashShape shape;
shape.m_radius0 = radiusScale0;
shape.m_radius1 = radiusScale1;
shape.m_deformFunc = 4;
shape.m_sphere0Pos = sphere0Pos;
shape.m_sphere1Pos = sphere1Pos;
shape.m_childTransform = compound->getChildTransform(0);
int graphicsShapeIndex = -1;
int* graphicsShapeIndexPtr = m_data->m_hashShapes[shape];
if (graphicsShapeIndexPtr)
{
graphicsShapeIndex = *graphicsShapeIndexPtr;
}
else
{
for (int i=0;i<numVertices;i++)
{
btVector3 vert;
vert.setValue(textured_detailed_sphere_vertices[i*9+0],
textured_detailed_sphere_vertices[i*9+1],
textured_detailed_sphere_vertices[i*9+2]);
btVector3 trVer(0,0,0);
if (vert.dot(fromTo)>0)
{
trVer = vert*radiusScale1;
trVer+=sphere1Pos;
trVer = compound->getChildTransform(0)*trVer;
} else
{
trVer = vert*radiusScale0;
trVer+=sphere0Pos;
trVer=compound->getChildTransform(0)*trVer;
}
transformedVertices[i*9+0] = trVer[0];
transformedVertices[i*9+1] = trVer[1];
transformedVertices[i*9+2] = trVer[2];
transformedVertices[i*9+3] =textured_detailed_sphere_vertices[i*9+3];
transformedVertices[i*9+4] =textured_detailed_sphere_vertices[i*9+4];
transformedVertices[i*9+5] =textured_detailed_sphere_vertices[i*9+5];
transformedVertices[i*9+6] =textured_detailed_sphere_vertices[i*9+6];
transformedVertices[i*9+7] =textured_detailed_sphere_vertices[i*9+7];
transformedVertices[i*9+8] =textured_detailed_sphere_vertices[i*9+8];
}
int numIndices = sizeof(textured_detailed_sphere_indices)/sizeof(int);
graphicsShapeIndex = registerGraphicsShape(&transformedVertices[0],numVertices,textured_detailed_sphere_indices,numIndices,B3_GL_TRIANGLES,m_data->m_checkedTextureGrey);
m_data->m_hashShapes.insert(shape,graphicsShapeIndex);
}
collisionShape->setUserIndex(graphicsShapeIndex);
return;
}
}
}
}
if (collisionShape->getShapeType()==CAPSULE_SHAPE_PROXYTYPE)
{
btCapsuleShape* sphereShape = (btCapsuleShape*) collisionShape;//Y up
int up = sphereShape->getUpAxis();
btScalar halfHeight = sphereShape->getHalfHeight();
btScalar radius = sphereShape->getRadius();
btScalar sphereSize = 2.*radius;
btVector3 radiusScale(sphereSize,sphereSize,sphereSize);
MyHashShape shape;
shape.m_radius0 = sphereSize;
shape.m_deformFunc = 3;
shape.m_upAxis = up;
shape.m_halfHeight = halfHeight;
int graphicsShapeIndex = -1;
int* graphicsShapeIndexPtr = m_data->m_hashShapes[shape];
if (graphicsShapeIndexPtr)
{
graphicsShapeIndex = *graphicsShapeIndexPtr;
}
else
{
btAlignedObjectArray<float> transformedVertices;
int numVertices = sizeof(textured_detailed_sphere_vertices)/strideInBytes;
transformedVertices.resize(numVertices*9);
for (int i=0;i<numVertices;i++)
{
btVector3 vert;
vert.setValue(textured_detailed_sphere_vertices[i*9+0],
textured_detailed_sphere_vertices[i*9+1],
textured_detailed_sphere_vertices[i*9+2]);
btVector3 trVer = radiusScale*vert;
if (trVer[up]>0)
trVer[up]+=halfHeight;
else
trVer[up]-=halfHeight;
transformedVertices[i*9+0] = trVer[0];
transformedVertices[i*9+1] = trVer[1];
transformedVertices[i*9+2] = trVer[2];
transformedVertices[i*9+3] =textured_detailed_sphere_vertices[i*9+3];
transformedVertices[i*9+4] =textured_detailed_sphere_vertices[i*9+4];
transformedVertices[i*9+5] =textured_detailed_sphere_vertices[i*9+5];
transformedVertices[i*9+6] =textured_detailed_sphere_vertices[i*9+6];
transformedVertices[i*9+7] =textured_detailed_sphere_vertices[i*9+7];
transformedVertices[i*9+8] =textured_detailed_sphere_vertices[i*9+8];
}
int numIndices = sizeof(textured_detailed_sphere_indices)/sizeof(int);
graphicsShapeIndex = registerGraphicsShape(&transformedVertices[0],numVertices,textured_detailed_sphere_indices,numIndices,B3_GL_TRIANGLES,m_data->m_checkedTextureGrey);
m_data->m_hashShapes.insert(shape,graphicsShapeIndex);
}
collisionShape->setUserIndex(graphicsShapeIndex);
return;
}
if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(collisionShape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 128;
btVector3 verts[4];
verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen;
verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen;
verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen;
verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen;
int startIndex = 0;
indices.push_back(startIndex+0);
indices.push_back(startIndex+1);
indices.push_back(startIndex+2);
indices.push_back(startIndex+0);
indices.push_back(startIndex+2);
indices.push_back(startIndex+3);
btTransform parentTransform;
parentTransform.setIdentity();
btVector3 triNormal = parentTransform.getBasis()*planeNormal;
gfxVertices.resize(4);
for (int i=0;i<4;i++)
{
btVector3 vtxPos;
btVector3 pos =parentTransform*verts[i];
gfxVertices[i].xyzw[0] = pos[0];
gfxVertices[i].xyzw[1] = pos[1];
gfxVertices[i].xyzw[2] = pos[2];
gfxVertices[i].xyzw[3] = 1;
gfxVertices[i].normal[0] = triNormal[0];
gfxVertices[i].normal[1] = triNormal[1];
gfxVertices[i].normal[2] = triNormal[2];
}
//verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen;
//verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen;
//verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen;
//verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen;
gfxVertices[0].uv[0] = vecLen/2;
gfxVertices[0].uv[1] = vecLen/2;
gfxVertices[1].uv[0] = -vecLen/2;
gfxVertices[1].uv[1] = vecLen/2;
gfxVertices[2].uv[0] = -vecLen/2;
gfxVertices[2].uv[1] = -vecLen/2;
gfxVertices[3].uv[0] = vecLen/2;
gfxVertices[3].uv[1] = -vecLen/2;
int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),B3_GL_TRIANGLES,m_data->m_checkedTexture);
collisionShape->setUserIndex(shapeId);
return;
}
btTransform startTrans;startTrans.setIdentity();
//todo: create some textured objects for popular objects, like plane, cube, sphere, capsule
{
btAlignedObjectArray<btVector3> vertexPositions;
@@ -587,7 +1162,7 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
sortedObjects.push_back(colObj);
}
sortedObjects.quickSort(shapePointerCompareFunc);
//sortedObjects.quickSort(shapePointerCompareFunc);
for (int i=0;i<sortedObjects.size();i++)
{
btCollisionObject* colObj = sortedObjects[i];
@@ -596,7 +1171,12 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
createCollisionShapeGraphicsObject(colObj->getCollisionShape());
int colorIndex = colObj->getBroadphaseHandle()->getUid() & 3;
btVector3 color= sColors[colorIndex];
btVector3 color;
color = sColors[colorIndex];
if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE)
{
color.setValue(1,1,1);
}
createCollisionObjectGraphicsObject(colObj,color);
}

View File

@@ -25,7 +25,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId);
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
virtual void removeAllGraphicsInstances();
virtual void removeGraphicsInstance(int graphicsUid);
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld);
@@ -89,6 +90,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void dumpFramesToVideo(const char* mp4FileName);
int createCheckeredTexture(int r,int g, int b);
};
#endif //OPENGL_GUI_HELPER_H

View File

@@ -35,6 +35,28 @@ project "App_BulletExampleBrowser"
}
end
if _OPTIONS["audio"] then
files {"../TinyAudio/*.cpp"}
defines {"B3_ENABLE_TINY_AUDIO"}
if os.is("Windows") then
links {"winmm","Wsock32","dsound"}
defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"}
end
if os.is("Linux") then initX11()
defines {"__OS_LINUX__","__LINUX_ALSA__"}
links {"asound","pthread"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
defines {"__OS_MACOSX__","__MACOSX_CORE__"}
end
end
if _OPTIONS["lua"] then
includedirs{"../ThirdPartyLibs/lua-5.2.3/src"}
links {"lua-5.2.3"}
@@ -184,6 +206,8 @@ project "BulletExampleBrowserLib"
files {"../LuaDemo/LuaPhysicsSetup.cpp"}
end
initOpenGL()
initGlew()

View File

@@ -36,6 +36,9 @@ static btScalar gBoxRestitution = 0; // set box restitution to 0
static btScalar gSphereFriction = 1; // set sphere friction to 1
static btScalar gSphereRollingFriction = 1; // set sphere rolling friction to 1
static btScalar gSphereSpinningFriction = 0.3; // set sphere spinning friction to 0.3
static btScalar gSphereRestitution = 0; // set sphere restitution to 0
@@ -149,6 +152,16 @@ void InclinedPlaneExample::initPhysics()
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{ // create slider to change the sphere rolling friction
SliderParams slider("Sphere Spinning",&gSphereSpinningFriction);
slider.m_minVal=0;
slider.m_maxVal=2;
slider.m_clampToNotches = false;
slider.m_callback = onSphereRestitutionChanged;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{ // create slider to change the sphere restitution
SliderParams slider("Sphere Restitution",&gSphereRestitution);
slider.m_minVal=0;
@@ -240,6 +253,8 @@ void InclinedPlaneExample::initPhysics()
gSphere->setFriction(gSphereFriction);
gSphere->setRestitution(gSphereRestitution);
gSphere->setRollingFriction(gSphereRollingFriction);
gSphere->setSpinningFriction(gSphereSpinningFriction);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

View File

@@ -1,6 +1,8 @@
#include "BulletMJCFImporter.h"
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "Bullet3Common/b3FileUtils.h"
#include "Bullet3Common/b3HashMap.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include <iostream>
@@ -129,6 +131,35 @@ struct MyMJCFAsset
std::string m_fileName;
};
struct MyMJCFDefaults
{
int m_defaultCollisionGroup;
int m_defaultCollisionMask;
btScalar m_defaultCollisionMargin;
// joint defaults
std::string m_defaultJointLimited;
// geom defaults
std::string m_defaultGeomRgba;
int m_defaultConDim;
double m_defaultLateralFriction;
double m_defaultSpinningFriction;
double m_defaultRollingFriction;
MyMJCFDefaults()
:m_defaultCollisionGroup(1),
m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001),//assume unit meters, margin is 1mm
m_defaultConDim(3),
m_defaultLateralFriction(0.5),
m_defaultSpinningFriction(0),
m_defaultRollingFriction(0)
{
}
};
struct BulletMJCFImporterInternalData
{
GUIHelperInterface* m_guiHelper;
@@ -149,30 +180,29 @@ struct BulletMJCFImporterInternalData
int m_activeModel;
int m_activeBodyUniqueId;
//todo: for full MJCF compatibility, we would need a stack of default values
int m_defaultCollisionGroup;
int m_defaultCollisionMask;
btScalar m_defaultCollisionMargin;
// joint defaults
std::string m_defaultJointLimited;
// geom defaults
std::string m_defaultGeomRgba;
//todo: for better MJCF compatibility, we would need a stack of default values
MyMJCFDefaults m_globalDefaults;
b3HashMap<b3HashString, MyMJCFDefaults> m_classDefaults;
//those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
BulletMJCFImporterInternalData()
:m_activeModel(-1),
m_activeBodyUniqueId(-1),
m_defaultCollisionGroup(1),
m_defaultCollisionMask(1),
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
m_activeBodyUniqueId(-1)
{
m_pathPrefix[0] = 0;
}
~BulletMJCFImporterInternalData()
{
for (int i=0;i<m_models.size();i++)
{
delete m_models[i];
}
}
std::string sourceFileLocation(TiXmlElement* e)
{
#if 0
@@ -246,7 +276,9 @@ struct BulletMJCFImporterInternalData
}
}
bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger)
bool parseDefaults(MyMJCFDefaults& defaults, TiXmlElement* root_xml, MJCFErrorLogger* logger)
{
bool handled= false;
//rudimentary 'default' support, would need more work for better feature coverage
@@ -254,6 +286,27 @@ struct BulletMJCFImporterInternalData
{
std::string n = child_xml->Value();
if (n.find("default")!=std::string::npos)
{
const char* className = child_xml->Attribute("class");
if (className)
{
MyMJCFDefaults* curDefaultsPtr = m_classDefaults[className];
if (!curDefaultsPtr)
{
MyMJCFDefaults def;
m_classDefaults.insert(className,def);
curDefaultsPtr = m_classDefaults[className];
}
if (curDefaultsPtr)
{
MyMJCFDefaults& curDefaults = *curDefaultsPtr;
parseDefaults(curDefaults, child_xml, logger);
}
}
}
if (n=="inertial")
{
}
@@ -269,7 +322,7 @@ struct BulletMJCFImporterInternalData
// limited="true"
if (const char* conTypeStr = child_xml->Attribute("limited"))
{
m_defaultJointLimited = child_xml->Attribute("limited");
defaults.m_defaultJointLimited = child_xml->Attribute("limited");
}
}
if (n=="geom")
@@ -278,24 +331,60 @@ struct BulletMJCFImporterInternalData
const char* conTypeStr = child_xml->Attribute("contype");
if (conTypeStr)
{
m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr);
defaults.m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr);
}
const char* conAffinityStr = child_xml->Attribute("conaffinity");
if (conAffinityStr)
{
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
defaults.m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
}
const char* rgba = child_xml->Attribute("rgba");
if (rgba)
{
m_defaultGeomRgba = rgba;
defaults.m_defaultGeomRgba = rgba;
}
const char* conDimS = child_xml->Attribute("condim");
if (conDimS)
{
defaults.m_defaultConDim=urdfLexicalCast<int>(conDimS);
}
int conDim = defaults.m_defaultConDim;
const char* frictionS = child_xml->Attribute("friction");
if (frictionS)
{
btArray<std::string> pieces;
btArray<float> frictions;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, frictionS, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
frictions.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (frictions.size()>0)
{
defaults.m_defaultLateralFriction = frictions[0];
}
if (frictions.size()>1)
{
defaults.m_defaultSpinningFriction = frictions[1];
}
if (frictions.size()>2)
{
defaults.m_defaultRollingFriction = frictions[2];
}
}
}
}
handled=true;
return handled;
}
bool parseRootLevel(TiXmlElement* root_xml,MJCFErrorLogger* logger)
bool parseRootLevel(MyMJCFDefaults& defaults, TiXmlElement* root_xml,MJCFErrorLogger* logger)
{
for (TiXmlElement* rootxml = root_xml->FirstChildElement() ; rootxml ; rootxml = rootxml->NextSiblingElement())
{
@@ -308,7 +397,7 @@ struct BulletMJCFImporterInternalData
int modelIndex = m_models.size();
UrdfModel* model = new UrdfModel();
m_models.push_back(model);
parseBody(rootxml,modelIndex, INVALID_LINK_INDEX,logger);
parseBody(defaults, rootxml,modelIndex, INVALID_LINK_INDEX,logger);
initTreeAndRoot(*model,logger);
handled = true;
}
@@ -336,7 +425,7 @@ struct BulletMJCFImporterInternalData
// modelPtr->m_rootLinks.push_back(linkPtr);
btVector3 inertialShift(0,0,0);
parseGeom(rootxml,modelIndex, linkIndex,logger,inertialShift);
parseGeom(defaults, rootxml,modelIndex, linkIndex,logger,inertialShift);
initTreeAndRoot(*modelPtr,logger);
handled = true;
@@ -355,7 +444,7 @@ struct BulletMJCFImporterInternalData
return true;
}
bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
bool parseJoint(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
{
bool jointHandled = false;
const char* jType = link_xml->Attribute("type");
@@ -399,7 +488,7 @@ struct BulletMJCFImporterInternalData
}
double range[2] = {1,0};
std::string lim = m_defaultJointLimited;
std::string lim = m_globalDefaults.m_defaultJointLimited;
if (limitedStr)
{
lim = limitedStr;
@@ -531,7 +620,7 @@ struct BulletMJCFImporterInternalData
*/
return false;
}
bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
bool parseGeom(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
{
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0)
@@ -549,13 +638,67 @@ struct BulletMJCFImporterInternalData
bool handledGeomType = false;
UrdfGeometry geom;
const char* gType = link_xml->Attribute("type");
const char* sz = link_xml->Attribute("size");
const char* posS = link_xml->Attribute("pos");
int conDim = defaults.m_defaultConDim;
std::string rgba = m_defaultGeomRgba;
const char* conDimS = link_xml->Attribute("condim");
{
if (conDimS)
{
conDim = urdfLexicalCast<int>(conDimS);
}
}
double lateralFriction = defaults.m_defaultLateralFriction;
double spinningFriction = defaults.m_defaultSpinningFriction;
double rollingFriction = defaults.m_defaultRollingFriction;
const char* frictionS = link_xml->Attribute("friction");
if (frictionS)
{
btArray<std::string> pieces;
btArray<float> frictions;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, frictionS, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
frictions.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (frictions.size()>0)
{
lateralFriction = frictions[0];
}
if (frictions.size()>1 && conDim>3)
{
spinningFriction = frictions[1];
}
if (frictions.size()>2 && conDim>4)
{
rollingFriction = frictions[2];
}
}
linkPtr->m_contactInfo.m_lateralFriction=lateralFriction;
linkPtr->m_contactInfo.m_spinningFriction=spinningFriction;
linkPtr->m_contactInfo.m_rollingFriction=rollingFriction;
if (conDim>3)
{
linkPtr->m_contactInfo.m_spinningFriction=defaults.m_defaultSpinningFriction;
linkPtr->m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
}
if (conDim>4)
{
linkPtr->m_contactInfo.m_rollingFriction=defaults.m_defaultRollingFriction;
linkPtr->m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
}
std::string rgba = defaults.m_defaultGeomRgba;
if (const char* rgbattr = link_xml->Attribute("rgba"))
{
rgba = rgbattr;
@@ -568,6 +711,7 @@ struct BulletMJCFImporterInternalData
geom.m_localMaterial.m_name = rgba;
}
const char* posS = link_xml->Attribute("pos");
if (posS)
{
btVector3 pos(0,0,0);
@@ -577,18 +721,32 @@ struct BulletMJCFImporterInternalData
linkLocalFrame.setOrigin(pos);
}
}
const char* ornS = link_xml->Attribute("quat");
if (ornS)
{
std::string ornStr = ornS;
btQuaternion orn(0,0,0,1);
btVector4 o4;
if (parseVector4(o4,ornStr))
if (parseVector4(o4, ornS))
{
orn.setValue(o4[1],o4[2],o4[3],o4[0]);
linkLocalFrame.setRotation(orn);
}
}
const char* axis_and_angle = link_xml->Attribute("axisangle");
if (axis_and_angle)
{
btQuaternion orn(0,0,0,1);
btVector4 o4;
if (parseVector4(o4, axis_and_angle))
{
orn.setRotation(btVector3(o4[0],o4[1],o4[2]), o4[3]);
linkLocalFrame.setRotation(orn);
}
}
const char* gType = link_xml->Attribute("type");
if (gType)
{
std::string geomType = gType;
@@ -653,8 +811,8 @@ struct BulletMJCFImporterInternalData
}
}
geom.m_capsuleRadius = 0;
geom.m_capsuleHeight = 0.f;
geom.m_capsuleRadius = 2.00f; // 2 to make it visible if something is wrong
geom.m_capsuleHeight = 2.00f;
if (sizes.size()>0)
{
@@ -717,10 +875,10 @@ struct BulletMJCFImporterInternalData
UrdfCollision col;
col.m_flags |= URDF_HAS_COLLISION_GROUP;
col.m_collisionGroup = m_defaultCollisionGroup;
col.m_collisionGroup = defaults.m_defaultCollisionGroup;
col.m_flags |= URDF_HAS_COLLISION_MASK;
col.m_collisionMask = m_defaultCollisionMask;
col.m_collisionMask = defaults.m_defaultCollisionMask;
//contype, conaffinity
const char* conTypeStr = link_xml->Attribute("contype");
@@ -738,6 +896,7 @@ struct BulletMJCFImporterInternalData
col.m_geometry = geom;
col.m_linkLocalFrame = linkLocalFrame;
col.m_sourceFileLocation = sourceFileLocation(link_xml);
linkPtr->m_collisionArray.push_back(col);
} else
@@ -811,22 +970,6 @@ struct BulletMJCFImporterInternalData
col->m_geometry.m_boxSize[2];
break;
}
case URDF_GEOM_CYLINDER:
{
double r = col->m_geometry.m_capsuleRadius;
btScalar h(0);
//and one cylinder of 'height'
if (col->m_geometry.m_hasFromTo)
{
h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length();
} else
{
h = col->m_geometry.m_capsuleHeight;
}
totalVolume += SIMD_PI*r*r*h;
break;
}
case URDF_GEOM_MESH:
{
//todo (based on mesh bounding box?)
@@ -837,11 +980,15 @@ struct BulletMJCFImporterInternalData
//todo
break;
}
case URDF_GEOM_CYLINDER:
case URDF_GEOM_CAPSULE:
{
//one sphere
double r = col->m_geometry.m_capsuleRadius;
totalVolume += 4./3.*SIMD_PI*r*r*r;
if (col->m_geometry.m_type==URDF_GEOM_CAPSULE)
{
totalVolume += 4./3.*SIMD_PI*r*r*r;
}
btScalar h(0);
if (col->m_geometry.m_hasFromTo)
{
@@ -878,9 +1025,9 @@ struct BulletMJCFImporterInternalData
UrdfModel* modelPtr = m_models[modelIndex];
int orgChildLinkIndex = modelPtr->m_links.size();
UrdfLink* linkPtr = new UrdfLink();
char uniqueLinkName[1024];
sprintf(uniqueLinkName,"link%d",orgChildLinkIndex );
linkPtr->m_name = uniqueLinkName;
char linkn[1024];
sprintf(linkn, "link%d_%d", modelIndex, orgChildLinkIndex);
linkPtr->m_name = linkn;
if (namePtr)
{
linkPtr->m_name = namePtr;
@@ -891,10 +1038,21 @@ struct BulletMJCFImporterInternalData
return orgChildLinkIndex;
}
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
bool parseBody(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
{
MyMJCFDefaults curDefaults = defaults;
int newParentLinkIndex = orgParentLinkIndex;
const char* childClassName = link_xml->Attribute("childclass");
if (childClassName)
{
MyMJCFDefaults* classDefaults = m_classDefaults[childClassName];
if (classDefaults)
{
curDefaults = *classDefaults;
}
}
const char* bodyName = link_xml->Attribute("name");
int orgChildLinkIndex = createBody(modelIndex,bodyName);
btTransform localInertialFrame;
@@ -998,7 +1156,7 @@ struct BulletMJCFImporterInternalData
}
int newLinkIndex = createBody(modelIndex,0);
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,linkTransform,jointTrans);
parseJoint(curDefaults,xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,linkTransform,jointTrans);
//getLink(modelIndex,newLinkIndex)->m_linkTransformInWorld = jointTrans*linkTransform;
@@ -1013,7 +1171,7 @@ struct BulletMJCFImporterInternalData
int newLinkIndex = createBody(modelIndex,0);
btTransform joint2nextjoint = jointTrans.inverse();
btTransform unused;
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused);
parseJoint(curDefaults, xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused);
newParentLinkIndex = newLinkIndex;
//todo: compute relative joint transforms (if any) and append to linkTransform
hasJoint = true;
@@ -1024,7 +1182,7 @@ struct BulletMJCFImporterInternalData
if (n == "geom")
{
btVector3 inertialShift(0,0,0);
parseGeom(xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
parseGeom(curDefaults, xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
if (!massDefined)
{
localInertialFrame.setOrigin(inertialShift);
@@ -1035,7 +1193,7 @@ struct BulletMJCFImporterInternalData
//recursive
if (n=="body")
{
parseBody(xml,modelIndex,orgChildLinkIndex,logger);
parseBody(curDefaults, xml,modelIndex,orgChildLinkIndex,logger);
handled = true;
}
@@ -1055,6 +1213,7 @@ struct BulletMJCFImporterInternalData
}
linkPtr->m_linkTransformInWorld = linkTransform;
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
{
//linkPtr->m_linkTransformInWorld.setIdentity();
@@ -1288,7 +1447,7 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("default"); link_xml; link_xml = link_xml->NextSiblingElement("default"))
{
m_data->parseDefaults(link_xml,logger);
m_data->parseDefaults(m_data->m_globalDefaults,link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler"))
@@ -1304,12 +1463,12 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
{
m_data->parseRootLevel(link_xml,logger);
m_data->parseRootLevel(m_data->m_globalDefaults, link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("worldbody"); link_xml; link_xml = link_xml->NextSiblingElement("worldbody"))
{
m_data->parseRootLevel(link_xml,logger);
m_data->parseRootLevel(m_data->m_globalDefaults, link_xml,logger);
}
@@ -1570,7 +1729,7 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::sha
}
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes( int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compound = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compound);
@@ -1655,7 +1814,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
childShape = MjcfCreateConvexHullFromShapes( shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
}
break;
@@ -1713,7 +1872,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
convexHull->setMargin(m_data->m_globalDefaults.m_defaultCollisionMargin);
childShape = convexHull;
}
}
@@ -1741,6 +1900,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
}
break;
}
case URDF_GEOM_UNKNOWN:
{
break;
}
} // switch geom
if (childShape)

View File

@@ -1134,9 +1134,9 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
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)
{
const UrdfLink* link = *linkPtr;
@@ -1146,6 +1146,22 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
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
{
if (m_data->m_customVisualShapesConverter)

View File

@@ -41,8 +41,10 @@ public:
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 void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;

View File

@@ -4,6 +4,7 @@
#include "MultiBodyCreationInterface.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
struct GUIHelperInterface;
class btMultiBody;
@@ -28,7 +29,6 @@ protected:
struct GUIHelperInterface* m_guiHelper;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
public:

View 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

View File

@@ -285,6 +285,9 @@ void ConvertURDF2BulletInternal(
if (!(flags & CUF_USE_URDF_INERTIA))
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
btAssert(localInertiaDiagonal[0] < 1e10);
btAssert(localInertiaDiagonal[1] < 1e10);
btAssert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
@@ -470,12 +473,12 @@ void ConvertURDF2BulletInternal(
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int colGroup=0, colMask=0;
int flags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
if (flags & URDF_HAS_COLLISION_GROUP)
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
}
if (flags & URDF_HAS_COLLISION_MASK)
if (collisionFlags & URDF_HAS_COLLISION_MASK)
{
collisionFilterMask = colMask;
}
@@ -495,6 +498,14 @@ void ConvertURDF2BulletInternal(
if (mbLinkIndex>=0) //???? double-check +/- 1
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_PARENT)
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
}
if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
}
} else
{
cache.m_bulletMultiBody->setBaseCollider(col);
@@ -536,7 +547,9 @@ void ConvertURDF2Bullet(
if (world1 && cache.m_bulletMultiBody)
{
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision(false);
mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0);
mb->finalizeMultiDof();
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];

View File

@@ -19,7 +19,9 @@ enum ConvertURDFFlags {
// Use inertia values in URDF instead of recomputing them from collision shape.
CUF_USE_URDF_INERTIA = 2,
CUF_USE_MJCF = 4,
CUF_USE_SELF_COLLISION=8
CUF_USE_SELF_COLLISION=8,
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16,
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
};
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,

View File

@@ -5,7 +5,7 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include "URDFJointTypes.h"
#include "SDFAudioTypes.h"
class URDFImporterInterface
{
@@ -40,6 +40,8 @@ public:
///this API will likely change, don't override it!
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;
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.

View File

@@ -13,48 +13,10 @@ m_activeSdfModel(-1)
UrdfParser::~UrdfParser()
{
cleanModel(&m_urdf2Model);
for (int i=0;i<m_tmpModels.size();i++)
{
cleanModel(m_tmpModels[i]);
for (int i=0;i<m_tmpModels.size();i++)
{
delete m_tmpModels[i];
}
m_sdfModels.clear();
m_tmpModels.clear();
}
void UrdfParser::cleanModel(UrdfModel* model)
{
for (int i=0;i<model->m_materials.size();i++)
{
UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
if (matPtr)
{
UrdfMaterial* mat = *matPtr;
delete mat;
}
}
for (int i=0;i<model->m_links.size();i++)
{
UrdfLink** linkPtr = model->m_links.getAtIndex(i);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
delete link;
}
}
for (int i=0;i<model->m_joints.size();i++)
{
UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
delete joint;
}
}
}
}
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
@@ -663,6 +625,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
TiXmlElement* ci = config->FirstChildElement("contact");
@@ -1395,6 +1433,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
delete material;
logger->reportWarning("Duplicate material");
} else
{
@@ -1418,6 +1457,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
delete link;
return false;
} else
{
@@ -1466,6 +1506,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
delete joint;
return false;
}
else
@@ -1476,6 +1517,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
else
{
logger->reportError("joint xml is not initialized correctly");
delete joint;
return false;
}
}
@@ -1590,7 +1632,8 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
logger->reportWarning("Duplicate material");
delete material;
} else
{
localModel->m_materials.insert(material->m_name.c_str(),material);
@@ -1613,6 +1656,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
delete link;
return false;
} else
{
@@ -1661,6 +1705,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
delete joint;
return false;
}
else
@@ -1671,6 +1716,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
else
{
logger->reportError("joint xml is not initialized correctly");
delete joint;
return false;
}
}

View File

@@ -5,6 +5,7 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
#include "URDFJointTypes.h"
#include "SDFAudioTypes.h"
#define btArray btAlignedObjectArray
#include <string>
@@ -50,9 +51,9 @@ enum UrdfGeomTypes
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
URDF_GEOM_MESH,
URDF_GEOM_PLANE,
URDF_GEOM_CAPSULE//non-standard URDF?
URDF_GEOM_PLANE,
URDF_GEOM_CAPSULE, //non-standard URDF?
URDF_GEOM_UNKNOWN,
};
@@ -83,6 +84,14 @@ struct UrdfGeometry
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;
UrdfGeometry()
:m_type(URDF_GEOM_UNKNOWN),
m_hasFromTo(false),
m_hasLocalMaterial(false)
{
}
};
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
@@ -132,6 +141,8 @@ struct UrdfLink
URDFLinkContactInfo m_contactInfo;
SDFAudioSource m_audioSource;
UrdfLink()
:m_parentLink(0),
m_parentJoint(0)
@@ -184,7 +195,37 @@ struct UrdfModel
{
m_rootTransformInWorld.setIdentity();
}
~UrdfModel()
{
for (int i = 0; i < m_materials.size(); i++)
{
UrdfMaterial** ptr = m_materials.getAtIndex(i);
if (ptr)
{
UrdfMaterial* t = *ptr;
delete t;
}
}
for (int i = 0; i < m_links.size(); i++)
{
UrdfLink** ptr = m_links.getAtIndex(i);
if (ptr)
{
UrdfLink* t = *ptr;
delete t;
}
}
for (int i = 0; i < m_joints.size(); i++)
{
UrdfJoint** ptr = m_joints.getAtIndex(i);
if (ptr)
{
UrdfJoint* t = *ptr;
delete t;
}
}
}
};
class UrdfParser
@@ -199,7 +240,6 @@ protected:
int m_activeSdfModel;
void cleanModel(UrdfModel* model);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);

View File

@@ -53,6 +53,8 @@ float shadowMapWorldSize=10;
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "LoadShader.h"
#include "GLInstanceRendererInternalData.h"
@@ -79,7 +81,6 @@ float shadowMapWorldSize=10;
static InternalDataRenderer* sData2;
GLint lineWidthRange[2]={1,1};
static b3Vector3 gLightPos=b3MakeVector3(-5,12,-4);
struct b3GraphicsInstance
{
@@ -91,7 +92,7 @@ struct b3GraphicsInstance
int m_numVertices;
int m_numGraphicsInstances;
b3AlignedObjectArray<int> m_tempObjectUids;
int m_instanceOffset;
int m_vertexArrayOffset;
int m_primitiveType;
@@ -145,7 +146,22 @@ struct InternalTextureHandle
int m_height;
};
struct b3PublicGraphicsInstanceData
{
int m_shapeIndex;
int m_internalInstanceIndex;
GLfloat m_position[4];
GLfloat m_orientation[4];
GLfloat m_color[4];
GLfloat m_scale[4];
void clear()
{
}
};
typedef b3PoolBodyHandle<b3PublicGraphicsInstanceData> b3PublicGraphicsInstance;
struct InternalDataRenderer : public GLInstanceRendererInternalData
{
@@ -156,6 +172,7 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
GLfloat m_projectionMatrix[16];
GLfloat m_viewMatrix[16];
b3Vector3 m_lightPos;
GLuint m_defaultTexturehandle;
b3AlignedObjectArray<InternalTextureHandle> m_textureHandles;
@@ -165,13 +182,18 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
GLuint m_renderFrameBuffer;
b3ResizablePool< b3PublicGraphicsInstance> m_publicGraphicsInstances;
InternalDataRenderer() :
m_activeCamera(&m_defaultCamera1),
m_shadowMap(0),
m_shadowTexture(0),
m_renderFrameBuffer(0)
{
m_lightPos=b3MakeVector3(-50,50,50);
//clear to zero to make it obvious if the matrix is used uninitialized
for (int i=0;i<16;i++)
{
@@ -282,6 +304,8 @@ void GLInstancingRenderer::removeAllInstances()
delete m_graphicsInstances[i];
}
m_graphicsInstances.clear();
m_data->m_publicGraphicsInstances.exitHandles();
m_data->m_publicGraphicsInstances.initHandles();
}
@@ -311,8 +335,13 @@ GLInstancingRenderer::~GLInstancingRenderer()
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int bodyUniqueId)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
b3Assert(srcIndex<m_data->m_totalNumInstances);
b3Assert(srcIndex>=0);
m_data->m_instance_positions_ptr[srcIndex*4+0]=position[0];
@@ -325,16 +354,16 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
m_data->m_instance_quaternion_ptr[srcIndex*4+2]=orientation[2];
m_data->m_instance_quaternion_ptr[srcIndex*4+3]=orientation[3];
/* m_data->m_instance_colors_ptr[srcIndex*4+0]=color[0];
m_data->m_instance_colors_ptr[srcIndex*4+1]=color[1];
m_data->m_instance_colors_ptr[srcIndex*4+2]=color[2];
m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
*/
}
void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation)
void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId, float* position, float* orientation)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
b3Assert(srcIndex<m_data->m_totalNumInstances);
b3Assert(srcIndex>=0);
position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
@@ -346,16 +375,23 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, floa
orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int bodyUniqueId)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
m_data->m_instance_colors_ptr[srcIndex*4+1]=float(color[1]);
m_data->m_instance_colors_ptr[srcIndex*4+2]=float(color[2]);
m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]);
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIndex)
void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int bodyUniqueId)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
m_data->m_instance_colors_ptr[srcIndex*4+0]=color[0];
m_data->m_instance_colors_ptr[srcIndex*4+1]=color[1];
@@ -363,25 +399,37 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIn
m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int bodyUniqueId)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int bodyUniqueId)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
}
void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex)
void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectUniqueId)
{
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
//glFlush();
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(objectUniqueId);
b3Assert(pg);
int objectIndex = pg->m_internalInstanceIndex;
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
//b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
int totalNumInstances= 0;
@@ -572,15 +620,92 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const double*
return registerGraphicsInstance(shapeIndex,pos,orn,color,scaling);
}
int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
void GLInstancingRenderer::rebuildGraphicsInstances()
{
b3Assert(shapeIndex == (m_graphicsInstances.size()-1));
b3Assert(m_graphicsInstances.size()<m_data->m_maxNumObjectCapacity-1);
m_data->m_totalNumInstances = 0;
b3AlignedObjectArray<int> usedObjects;
m_data->m_publicGraphicsInstances.getUsedHandles(usedObjects);
for (int i=0;i<usedObjects.size();i++)
{
int bodyUniqueId = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
pg->m_position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
pg->m_position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
pg->m_position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
pg->m_orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0];
pg->m_orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1];
pg->m_orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
pg->m_orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
pg->m_color[0] = m_data->m_instance_colors_ptr[srcIndex*4+0];
pg->m_color[1] = m_data->m_instance_colors_ptr[srcIndex*4+1];
pg->m_color[2] = m_data->m_instance_colors_ptr[srcIndex*4+2];
pg->m_color[3] = m_data->m_instance_colors_ptr[srcIndex*4+3];
pg->m_scale[0] = m_data->m_instance_scale_ptr[srcIndex*3+0];
pg->m_scale[1] = m_data->m_instance_scale_ptr[srcIndex*3+1];
pg->m_scale[2] = m_data->m_instance_scale_ptr[srcIndex*3+2];
}
for (int i=0;i<m_graphicsInstances.size();i++)
{
m_graphicsInstances[i]->m_numGraphicsInstances = 0;
m_graphicsInstances[i]->m_instanceOffset = 0;
m_graphicsInstances[i]->m_tempObjectUids.clear();
}
for (int i=0;i<usedObjects.size();i++)
{
int bodyUniqueId = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(bodyUniqueId);
}
int curOffset = 0;
m_data->m_totalNumInstances = 0;
for (int i=0;i<m_graphicsInstances.size();i++)
{
m_graphicsInstances[i]->m_instanceOffset = curOffset;
m_graphicsInstances[i]->m_numGraphicsInstances = 0;
for (int g=0;g<m_graphicsInstances[i]->m_tempObjectUids.size();g++)
{
curOffset++;
int objectUniqueId = m_graphicsInstances[i]->m_tempObjectUids[g];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(objectUniqueId);
registerGraphicsInstanceInternal(objectUniqueId,pg->m_position,pg->m_orientation,pg->m_color,pg->m_scale);
}
}
}
void GLInstancingRenderer::removeGraphicsInstance(int instanceUid)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(instanceUid);
b3Assert(pg);
if (pg)
{
m_data->m_publicGraphicsInstances.freeHandle(instanceUid);
rebuildGraphicsInstances();
}
}
int GLInstancingRenderer::registerGraphicsInstanceInternal(int newUid, const float* position, const float* quaternion, const float* color, const float* scaling)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(newUid);
int shapeIndex = pg->m_shapeIndex;
// b3Assert(pg);
// int objectIndex = pg->m_internalInstanceIndex;
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
int index = gfxObj->m_numGraphicsInstances + gfxObj->m_instanceOffset;
pg->m_internalInstanceIndex = index;
int maxElements = m_data->m_instance_positions_ptr.size();
if (index*4<maxElements)
@@ -611,7 +736,50 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float*
b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
return -1;
}
return index;//gfxObj->m_numGraphicsInstances;
return newUid;//gfxObj->m_numGraphicsInstances;
}
int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
int newUid = m_data->m_publicGraphicsInstances.allocHandle();
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(newUid);
pg->m_shapeIndex = shapeIndex;
//b3Assert(shapeIndex == (m_graphicsInstances.size()-1));
b3Assert(m_graphicsInstances.size()<m_data->m_maxNumObjectCapacity-1);
if (shapeIndex == (m_graphicsInstances.size()-1))
{
registerGraphicsInstanceInternal(newUid, position,quaternion,color,scaling);
} else
{
int srcIndex = m_data->m_totalNumInstances++;
pg->m_internalInstanceIndex = srcIndex;
m_data->m_instance_positions_ptr[srcIndex*4+0] = position[0];
m_data->m_instance_positions_ptr[srcIndex*4+1] = position[1];
m_data->m_instance_positions_ptr[srcIndex*4+2] = position[2];
m_data->m_instance_positions_ptr[srcIndex*4+3] = 1.;
m_data->m_instance_quaternion_ptr[srcIndex*4+0] = quaternion[0];
m_data->m_instance_quaternion_ptr[srcIndex*4+1] = quaternion[1];
m_data->m_instance_quaternion_ptr[srcIndex*4+2] = quaternion[2];
m_data->m_instance_quaternion_ptr[srcIndex*4+3] = quaternion[3];
m_data->m_instance_colors_ptr[srcIndex*4+0] = color[0];
m_data->m_instance_colors_ptr[srcIndex*4+1] = color[1];
m_data->m_instance_colors_ptr[srcIndex*4+2] = color[2];
m_data->m_instance_colors_ptr[srcIndex*4+3] = color[3];
m_data->m_instance_scale_ptr[srcIndex*3+0] = scaling[0];
m_data->m_instance_scale_ptr[srcIndex*3+1] = scaling[1];
m_data->m_instance_scale_ptr[srcIndex*3+2] = scaling[2];
rebuildGraphicsInstances();
}
return newUid;
}
@@ -996,23 +1164,26 @@ void GLInstancingRenderer::setActiveCamera(CommonCameraInterface* cam)
m_data->m_activeCamera = cam;
}
void GLInstancingRenderer::setLightPosition(const float lightPos[3])
{
m_data->m_lightPos[0] = lightPos[0];
m_data->m_lightPos[1] = lightPos[1];
m_data->m_lightPos[2] = lightPos[2];
}
void GLInstancingRenderer::setLightPosition(const double lightPos[3])
{
m_data->m_lightPos[0] = lightPos[0];
m_data->m_lightPos[1] = lightPos[1];
m_data->m_lightPos[2] = lightPos[2];
}
void GLInstancingRenderer::updateCamera(int upAxis)
{
b3Assert(glGetError() ==GL_NO_ERROR);
m_upAxis = upAxis;
switch (upAxis)
{
case 1:
gLightPos = b3MakeVector3(-50.f,100,30);
break;
case 2:
gLightPos = b3MakeVector3(-50.f,30,100);
break;
default:
b3Assert(0);
};
m_data->m_activeCamera->setCameraUpAxis(upAxis);
m_data->m_activeCamera->setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
@@ -1554,9 +1725,10 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
//float upf[3];
//m_data->m_activeCamera->getCameraUpVector(upf);
b3Vector3 up, fwd;
b3PlaneSpace1(gLightPos,up,fwd);
b3Vector3 lightDir = m_data->m_lightPos.normalized();
b3PlaneSpace1(lightDir,up,fwd);
// b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]);
b3CreateLookAt(gLightPos,center,up,&depthViewMatrix[0][0]);
b3CreateLookAt(m_data->m_lightPos,center,up,&depthViewMatrix[0][0]);
//b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2);
GLfloat depthModelMatrix[4][4];
@@ -1726,7 +1898,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
b3Vector3 gLightDir = gLightPos;
b3Vector3 gLightDir = m_data->m_lightPos;
gLightDir.normalize();
glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
@@ -1763,7 +1935,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
float MVP[16];
b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
b3Vector3 gLightDir = gLightPos;
b3Vector3 gLightDir = m_data->m_lightPos;
gLightDir.normalize();
glUniform3f(useShadow_lightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]);

View File

@@ -41,10 +41,10 @@ class GLInstancingRenderer : public CommonRenderInterface
int m_upAxis;
bool m_enableBlend;
int registerGraphicsInstanceInternal(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
void rebuildGraphicsInstances();
public:
GLInstancingRenderer(int m_maxObjectCapacity, int maxShapeCapacityInBytes = 56*1024*1024);
virtual ~GLInstancingRenderer();
@@ -57,7 +57,8 @@ public:
void InitShaders();
void CleanupShaders();
virtual void removeAllInstances();
virtual void removeGraphicsInstance(int instanceUid);
virtual void updateShape(int shapeIndex, const float* vertices);
///vertices must be in the format x,y,z, nx,ny,nz, u,v
@@ -72,6 +73,7 @@ public:
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling);
void writeTransforms();
@@ -117,7 +119,9 @@ public:
virtual CommonCameraInterface* getActiveCamera();
virtual void setActiveCamera(CommonCameraInterface* cam);
virtual void setLightPosition(const float lightPos[3]);
virtual void setLightPosition(const double lightPos[3]);
virtual void resize(int width, int height);
virtual int getScreenWidth()
{

View File

@@ -39,7 +39,9 @@ void main(void)
float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));
if (intensity<0.5)
visibility = 0;
intensity = 0.7*intensity + 0.3*intensity*visibility;
cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;

View File

@@ -32,7 +32,8 @@ static const char* useShadowMapInstancingFragmentShader= \
" \n"
" \n"
" float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));\n"
" \n"
" if (intensity<0.5)\n"
" visibility = 0;\n"
" intensity = 0.7*intensity + 0.3*intensity*visibility;\n"
" \n"
" cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"

File diff suppressed because it is too large Load Diff

View File

@@ -6,6 +6,7 @@
#include "GLInstanceGraphicsShape.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3ResizablePool.h"
B3_ATTRIBUTE_ALIGNED16(struct) SimpleGL2Shape
{
@@ -27,6 +28,9 @@ B3_ATTRIBUTE_ALIGNED16(struct) SimpleGL2Instance
b3Quaternion orn;
b3Vector3 m_rgbColor;
b3Vector3 m_scaling;
void clear()
{
}
};
@@ -38,13 +42,18 @@ struct InternalTextureHandle2
int m_height;
};
typedef b3PoolBodyHandle<SimpleGL2Instance> SimpleGL2InstanceHandle;
struct SimpleOpenGL2RendererInternalData
{
int m_width;
int m_height;
SimpleCamera m_camera;
b3AlignedObjectArray<SimpleGL2Shape*> m_shapes;
b3AlignedObjectArray<SimpleGL2Instance> m_graphicsInstances;
//b3AlignedObjectArray<SimpleGL2Instance> m_graphicsInstances1;
b3ResizablePool<SimpleGL2InstanceHandle> m_graphicsInstancesPool;
b3AlignedObjectArray<InternalTextureHandle2> m_textureHandles;
};
@@ -78,6 +87,14 @@ void SimpleOpenGL2Renderer::setActiveCamera(CommonCameraInterface* cam)
b3Assert(0);//not supported yet
}
void SimpleOpenGL2Renderer::setLightPosition(const float lightPos[3])
{
}
void SimpleOpenGL2Renderer::setLightPosition(const double lightPos[3])
{
}
void SimpleOpenGL2Renderer::updateCamera(int upAxis)
{
float projection[16];
@@ -110,11 +127,17 @@ void SimpleOpenGL2Renderer::removeAllInstances()
delete m_data->m_shapes[i];
}
m_data->m_shapes.clear();
m_data->m_graphicsInstances.clear();
m_data->m_graphicsInstancesPool.exitHandles();
m_data->m_graphicsInstancesPool.initHandles();
//also destroy textures?
m_data->m_textureHandles.clear();
}
void SimpleOpenGL2Renderer::removeGraphicsInstance(int instanceUid)
{
m_data->m_graphicsInstancesPool.freeHandle(instanceUid);
}
void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(float* color, int srcIndex)
{
@@ -134,7 +157,7 @@ void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int src
int SimpleOpenGL2Renderer::getTotalNumInstances() const
{
return m_data->m_graphicsInstances.size();
return m_data->m_graphicsInstancesPool.getNumHandles();
}
void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
@@ -150,7 +173,13 @@ void SimpleOpenGL2Renderer::getCameraProjectionMatrix(float projMat[16]) const
void SimpleOpenGL2Renderer::drawOpenGL(int instanceIndex)
{
const SimpleGL2Instance& inst = m_data->m_graphicsInstances[instanceIndex];
const SimpleGL2Instance* instPtr = m_data->m_graphicsInstancesPool.getHandle(instanceIndex);
if (0==instPtr)
{
b3Assert(0);
return;
}
const SimpleGL2Instance& inst = *instPtr;
const SimpleGL2Shape* shape = m_data->m_shapes[inst.m_shapeIndex];
@@ -236,9 +265,11 @@ void SimpleOpenGL2Renderer::drawOpenGL(int instanceIndex)
void SimpleOpenGL2Renderer::drawSceneInternal(int pass, int cameraUpAxis)
{
for (int i=0;i<m_data->m_graphicsInstances.size();i++)
b3AlignedObjectArray<int> usedHandles;
m_data->m_graphicsInstancesPool.getUsedHandles(usedHandles);
for (int i=0;i<usedHandles.size();i++)
{
drawOpenGL(i);
drawOpenGL(usedHandles[i]);
}
#if 0
@@ -427,8 +458,12 @@ void SimpleOpenGL2Renderer::activateTexture(int textureIndex)
int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling)
{
int sz = m_data->m_graphicsInstances.size();
SimpleGL2Instance& instance = m_data->m_graphicsInstances.expand();
int newHandle = m_data->m_graphicsInstancesPool.allocHandle();
// int sz = m_data->m_graphicsInstances.size();
SimpleGL2Instance& instance = *m_data->m_graphicsInstancesPool.getHandle(newHandle);
instance.m_shapeIndex = shapeIndex;
instance.m_position[0] = position[0];
instance.m_position[1] = position[1];
@@ -443,13 +478,13 @@ int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double
instance.m_scaling[0] = scaling[0];
instance.m_scaling[1] = scaling[1];
instance.m_scaling[2] = scaling[2];
return sz;
return newHandle;
}
int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
int sz = m_data->m_graphicsInstances.size();
SimpleGL2Instance& instance = m_data->m_graphicsInstances.expand();
int newHandle = m_data->m_graphicsInstancesPool.allocHandle();
SimpleGL2Instance& instance = *m_data->m_graphicsInstancesPool.getHandle(newHandle);
instance.m_shapeIndex = shapeIndex;
instance.m_position[0] = position[0];
instance.m_position[1] = position[1];
@@ -464,7 +499,7 @@ int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float*
instance.m_scaling[0] = scaling[0];
instance.m_scaling[1] = scaling[1];
instance.m_scaling[2] = scaling[2];
return sz;
return newHandle;
}
void SimpleOpenGL2Renderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)
@@ -537,26 +572,30 @@ int SimpleOpenGL2Renderer::registerShape(const float* vertices, int numvertices,
void SimpleOpenGL2Renderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
{
m_data->m_graphicsInstances[srcIndex].m_position[0] = position[0];
m_data->m_graphicsInstances[srcIndex].m_position[1] = position[1];
m_data->m_graphicsInstances[srcIndex].m_position[2] = position[2];
SimpleGL2Instance& graphicsInstance = *m_data->m_graphicsInstancesPool.getHandle(srcIndex);
graphicsInstance.m_position[0] = position[0];
graphicsInstance.m_position[1] = position[1];
graphicsInstance.m_position[2] = position[2];
m_data->m_graphicsInstances[srcIndex].orn[0] = orientation[0];
m_data->m_graphicsInstances[srcIndex].orn[1] = orientation[1];
m_data->m_graphicsInstances[srcIndex].orn[2] = orientation[2];
m_data->m_graphicsInstances[srcIndex].orn[3] = orientation[3];
graphicsInstance.orn[0] = orientation[0];
graphicsInstance.orn[1] = orientation[1];
graphicsInstance.orn[2] = orientation[2];
graphicsInstance.orn[3] = orientation[3];
}
void SimpleOpenGL2Renderer::writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
{
m_data->m_graphicsInstances[srcIndex].m_position[0] = position[0];
m_data->m_graphicsInstances[srcIndex].m_position[1] = position[1];
m_data->m_graphicsInstances[srcIndex].m_position[2] = position[2];
SimpleGL2Instance& graphicsInstance = *m_data->m_graphicsInstancesPool.getHandle(srcIndex);
graphicsInstance.m_position[0] = position[0];
graphicsInstance.m_position[1] = position[1];
graphicsInstance.m_position[2] = position[2];
m_data->m_graphicsInstances[srcIndex].orn[0] = orientation[0];
m_data->m_graphicsInstances[srcIndex].orn[1] = orientation[1];
m_data->m_graphicsInstances[srcIndex].orn[2] = orientation[2];
m_data->m_graphicsInstances[srcIndex].orn[3] = orientation[3];
graphicsInstance.orn[0] = orientation[0];
graphicsInstance.orn[1] = orientation[1];
graphicsInstance.orn[2] = orientation[2];
graphicsInstance.orn[3] = orientation[3];
}
void SimpleOpenGL2Renderer::writeTransforms()
{

View File

@@ -25,10 +25,14 @@ public:
virtual CommonCameraInterface* getActiveCamera();
virtual void setActiveCamera(CommonCameraInterface* cam);
virtual void setLightPosition(const float lightPos[3]);
virtual void setLightPosition(const double lightPos[3]);
virtual void resize(int width, int height);
virtual void removeAllInstances();
virtual void removeGraphicsInstance(int instanceUid);
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);

View File

@@ -258,7 +258,7 @@ LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
break;
};
}
if (keycode>=0 && sData && sData->m_keyboardCallback)// && ((HIWORD(lParam) & KF_REPEAT) == 0))
if (keycode>=0 && sData && sData->m_keyboardCallback && ((HIWORD(lParam) & KF_REPEAT) == 0))
{
int state = 1;
(*sData->m_keyboardCallback)(keycode,state);

View File

@@ -18,6 +18,8 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
struct TinyRendererSetupInternalData
{
@@ -42,6 +44,8 @@ struct TinyRendererSetupInternalData
int m_textureHandle;
int m_animateRenderer;
btVector3 m_lightPos;
TinyRendererSetupInternalData(int width, int height)
:
m_rgbColorBuffer(width,height,TGAImage::RGB),
@@ -53,6 +57,8 @@ struct TinyRendererSetupInternalData
m_textureHandle(0),
m_animateRenderer(0)
{
m_lightPos.setValue(-3,15,15);
m_depthBuffer.resize(m_width*m_height);
m_shadowBuffer.resize(m_width*m_height);
// m_segmentationMaskBuffer.resize(m_width*m_height);
@@ -115,9 +121,7 @@ struct TinyRendererSetup : public CommonExampleInterface
virtual bool keyboardCallback(int key, int state);
virtual void renderScene()
{
}
virtual void renderScene();
void animateRenderer(int animateRendererIndex)
{
@@ -295,6 +299,28 @@ void TinyRendererSetup::initPhysics()
m_guiHelper->getParameterInterface()->registerComboBox( comboParams);
}
{
SliderParams slider("LightPosX",&m_internalData->m_lightPos[0]);
slider.m_minVal=-10;
slider.m_maxVal=10;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("LightPosY",&m_internalData->m_lightPos[1]);
slider.m_minVal=-10;
slider.m_maxVal=10;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("LightPosZ",&m_internalData->m_lightPos[2]);
slider.m_minVal=-10;
slider.m_maxVal=10;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
@@ -303,14 +329,33 @@ void TinyRendererSetup::exitPhysics()
}
void TinyRendererSetup::stepSimulation(float deltaTime)
{
m_internalData->updateTransforms();
}
void TinyRendererSetup::renderScene()
{
m_internalData->updateTransforms();
btVector4 from(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2],1);
btVector4 toX(m_internalData->m_lightPos[0]+0.1,m_internalData->m_lightPos[1],m_internalData->m_lightPos[2],1);
btVector4 toY(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1]+0.1,m_internalData->m_lightPos[2],1);
btVector4 toZ(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]+0.1,1);
btVector4 colorX(1,0,0,1);
btVector4 colorY(0,1,0,1);
btVector4 colorZ(0,0,1,1);
int width=2;
m_guiHelper->getRenderInterface()->drawLine( from,toX,colorX,width);
m_guiHelper->getRenderInterface()->drawLine( from,toY,colorY,width);
m_guiHelper->getRenderInterface()->drawLine( from,toZ,colorZ,width);
if (!m_useSoftware)
{
btVector3 lightPos(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]);
m_guiHelper->getRenderInterface()->setLightPosition(lightPos);
for (int i=0;i<m_internalData->m_transforms.size();i++)
{
m_guiHelper->getRenderInterface()->writeSingleInstanceTransformToCPU(m_internalData->m_transforms[i].getOrigin(),m_internalData->m_transforms[i].getRotation(),i);
@@ -358,17 +403,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
btVector3 lightDirWorld;
switch (m_app->getUpAxis())
{
case 1:
lightDirWorld = btVector3(-50.f,100,30);
break;
case 2:
lightDirWorld = btVector3(-50.f,30,100);
break;
default:{}
};
btVector3 lightDirWorld = btVector3(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]);
m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();
@@ -399,17 +434,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
btVector3 lightDirWorld;
switch (m_app->getUpAxis())
{
case 1:
lightDirWorld = btVector3(-50.f,100,30);
break;
case 2:
lightDirWorld = btVector3(-50.f,30,100);
break;
default:{}
};
btVector3 lightDirWorld = btVector3(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]);
m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();

View File

@@ -140,4 +140,3 @@ int main(int argc, char* argv[])
}

View File

@@ -0,0 +1,351 @@
//VR Glove hand simulator is a C++ conversion from the Python pybullet vrhand_vive_tracker.py
//For more details about the VR glove, see also https://docs.google.com/document/d/1_qwXJRBTGKmhktdBtVQ6zdOdxwud1K30jt0G5dkAr10/edit
#include "b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include <string.h>
#include <stdio.h>
#include <assert.h>
#include "serial/serial.h"
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
double convertSensor(int inputV, int minV, int maxV)
{
b3Clamp(inputV,minV,maxV);
double outVal =(double)inputV;
double b = (outVal-(double)minV)/float(maxV-minV);
return (b);
}
void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int linkIndex, double targetPosition)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = 50.;
controlArgs.m_targetPosition = targetPosition;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(obUid,linkIndex,controlArgs);
}
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
std::string port="COM9";
args.GetCmdLineArgument("port",port);
int baud = 115200;
args.GetCmdLineArgument("speed",baud);
std::string mode = "SHARED_MEMORY";
args.GetCmdLineArgument("mode",mode);
int disableGui = 0;
args.GetCmdLineArgument("disableGui",disableGui);
int disableShadows = 0;
args.GetCmdLineArgument("disableShadows",disableShadows);
int useKitchen = 0;
args.GetCmdLineArgument("useKitchen",useKitchen);
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
args.GetCmdLineArgument("deviceTypeFilter",deviceTypeFilter);
printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud,mode.c_str());
b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
//Can also use eCONNECT_UDP,eCONNECT_TCP, for example: sim->connect(eCONNECT_UDP, "localhost", 1234);
if (mode=="GUI")
{
sim->connect(eCONNECT_GUI);
} else
{
if (mode=="DIRECT")
{
sim->connect(eCONNECT_DIRECT);
} else
{
sim->connect(eCONNECT_SHARED_MEMORY);
}
}
sim->setRealTimeSimulation(true);
sim->setInternalSimFlags(0);
sim->resetSimulation();
if (disableGui)
{
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
}
if (disableShadows)
{
sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);
}
sim->setTimeOut(12345);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
b3Scalar fixedTimeStep = 1./240.;
sim->setTimeStep(fixedTimeStep);
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
b3Vector3 rpy;
rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(b3MakeVector3(0,0,-9.8));
sim->setContactBreakingThreshold(0.001);
if (useKitchen)
{
b3RobotSimulatorLoadFileResults res;
sim->loadSDF("kitchens/1.sdf",res);
} else
{
sim->loadURDF("plane_with_collision_audio.urdf");
}
int handUid = -1;
b3RobotSimulatorLoadFileResults mjcfResults;
const char* mjcfFileName = "MPL/mpl2.xml";
if (sim->loadMJCF(mjcfFileName,mjcfResults))
{
printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size());
if (mjcfResults.m_uniqueObjectIds.size()==1)
{
handUid = mjcfResults.m_uniqueObjectIds[0];
}
}
if (handUid<0)
{
printf("Cannot load MJCF file %s\n", mjcfFileName);
}
#ifdef TOUCH
b3Vector3 handPos = b3MakeVector3(-0.10,-0.03,0.02);
b3Vector3 rollPitchYaw = b3MakeVector3(0.5*B3_PI,0,1.25*B3_PI);//-B3_PI/2,0,B3_PI/2);
handOrn = sim->getQuaternionFromEuler(rollPitchYaw);
#else
b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14,-3.14/2,0));
b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02);
#endif
b3Vector3 handStartPosWorld = b3MakeVector3(0.500000,0.300006,0.900000);
b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity();
b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
printf("handStartOrnWorld=%f,%f,%f,%f\n",handStartOrnWorld[0],handStartOrnWorld[1],handStartOrnWorld[2],handStartOrnWorld[3]);
jointInfo.m_childFrame[0] = handStartPosWorld[0];
jointInfo.m_childFrame[1] = handStartPosWorld[1];
jointInfo.m_childFrame[2] = handStartPosWorld[2];
jointInfo.m_childFrame[3] = handStartOrnWorld[0];
jointInfo.m_childFrame[4] = handStartOrnWorld[1];
jointInfo.m_childFrame[5] = handStartOrnWorld[2];
jointInfo.m_childFrame[6] = handStartOrnWorld[3];
jointInfo.m_parentFrame[0] = handPos[0];
jointInfo.m_parentFrame[1] = handPos[1];
jointInfo.m_parentFrame[2] = handPos[2];
jointInfo.m_parentFrame[3] = handOrn[0];
jointInfo.m_parentFrame[4] = handOrn[1];
jointInfo.m_parentFrame[5] = handOrn[2];
jointInfo.m_parentFrame[6] = handOrn[3];
sim->resetBasePositionAndOrientation(handUid,handStartPosWorld,handStartOrnWorld);
int handConstraintId = sim->createConstraint(handUid,-1,-1,-1,&jointInfo);
double maxFingerForce = 10;
double maxArmForce = 1000;
for (int j=0; j<sim->getNumJoints(handUid);j++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = maxFingerForce ;
controlArgs.m_kp = 0.1;
controlArgs.m_kd = 1;
controlArgs.m_targetPosition = 0;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(handUid,j,controlArgs);
}
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("table/table.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.200000,0.000000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("cube_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3( 0.950000,-0.100000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("sphere_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000,-0.400000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.;
int vidLogId = -1;
int minitaurLogId = -1;
int rotateCamera = 0;
serial::Serial my_serial;
try
{
// port, baudrate, timeout in milliseconds
my_serial.setBaudrate(baud);
my_serial.setPort(port);
my_serial.setBytesize(serial::sevenbits);
my_serial.setParity(serial::parity_odd);
my_serial.setStopbits(serial::stopbits_two);
my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01));
my_serial.open();
} catch(...)
{
printf("Cannot open port, use --port=PORTNAME\n");
exit(0);
}
if (!my_serial.isOpen())
{
printf("Cannot open serial port\n");
return -1;
}
my_serial.flush();
while (sim->canSubmitCommand())
{
clock.usleep(1);
b3VREventsData vrEvents;
sim->getVREvents(&vrEvents, deviceTypeFilter);
//instead of iterating over all vr events, we just take the most up-to-date one
if (vrEvents.m_numControllerEvents)
{
int i = vrEvents.m_numControllerEvents - 1;
b3VRControllerEvent& e = vrEvents.m_controllerEvents[i];
// printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]);
b3JointInfo changeConstraintInfo;
changeConstraintInfo.m_flags = 0;
changeConstraintInfo.m_jointMaxForce = maxArmForce ;
changeConstraintInfo.m_flags |= eJointChangeMaxForce;
changeConstraintInfo.m_childFrame[0] = e.m_pos[0];
changeConstraintInfo.m_childFrame[1] = e.m_pos[1];
changeConstraintInfo.m_childFrame[2] = e.m_pos[2];
changeConstraintInfo.m_flags |= eJointChangeChildFramePosition;
changeConstraintInfo.m_childFrame[3] = e.m_orn[0];
changeConstraintInfo.m_childFrame[4] = e.m_orn[1];
changeConstraintInfo.m_childFrame[5] = e.m_orn[2];
changeConstraintInfo.m_childFrame[6] = e.m_orn[3];
changeConstraintInfo.m_flags |= eJointChangeChildFrameOrientation;
sim->changeConstraint(handConstraintId, &changeConstraintInfo);
}
//read the serial output from the hand, extract into parts
std::string result;
try
{
result = my_serial.readline();
} catch(...)
{
}
if (result.length())
{
my_serial.flush();
int res = result.find("\n");
while (res<0)
{
result += my_serial.readline();
res = result.find("\n");
}
btAlignedObjectArray<std::string> pieces;
btAlignedObjectArray<std::string> separators;
separators.push_back(",");
urdfStringSplit( pieces, result, separators);
//printf("serial: %s\n", result.c_str());
if (pieces.size()==6)
{
double pinkTarget = 0;
double middleTarget = 0;
double indexTarget = 0;
double thumbTarget = 0;
{
int pink = atoi(pieces[1].c_str());
int middle = atoi(pieces[2].c_str());
int index = atoi(pieces[3].c_str());
int thumb = atoi(pieces[4].c_str());
pinkTarget = convertSensor(pink,250,400);
middleTarget = convertSensor(middle,250,400);
indexTarget = convertSensor(index,250,400);
thumbTarget = convertSensor(thumb,250,400);
}
//printf("pink = %d, middle=%d, index=%d, thumb=%d\n", pink,middle,index,thumb);
setJointMotorPositionControl(sim,handUid,5,1.3);
setJointMotorPositionControl(sim,handUid,7,thumbTarget);
setJointMotorPositionControl(sim,handUid,9,thumbTarget);
setJointMotorPositionControl(sim,handUid,11,thumbTarget);
setJointMotorPositionControl(sim,handUid,15,indexTarget);
setJointMotorPositionControl(sim,handUid,17,indexTarget);
setJointMotorPositionControl(sim,handUid,19,indexTarget);
setJointMotorPositionControl(sim,handUid,22,middleTarget);
setJointMotorPositionControl(sim,handUid,24,middleTarget);
setJointMotorPositionControl(sim,handUid,27,middleTarget);
double ringTarget = 0.5f*(pinkTarget+middleTarget);
setJointMotorPositionControl(sim,handUid,30,ringTarget);
setJointMotorPositionControl(sim,handUid,32,ringTarget);
setJointMotorPositionControl(sim,handUid,34,ringTarget);
setJointMotorPositionControl(sim,handUid,38,pinkTarget);
setJointMotorPositionControl(sim,handUid,40,pinkTarget);
setJointMotorPositionControl(sim,handUid,42,pinkTarget);
}
}
b3KeyboardEventsData keyEvents;
sim->getKeyboardEvents(&keyEvents);
//sim->stepSimulation();
if (rotateCamera)
{
static double yaw=0;
double distance = 1;
yaw+=0.1;
b3Vector3 basePos;
b3Quaternion baseOrn;
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
}
//b3Clock::usleep(1000.*1000.*fixedTimeStep);
}
printf("serial.close()\n");
my_serial.close();
printf("sim->disconnect\n");
sim->disconnect();
printf("delete sim\n");
delete sim;
printf("exit\n");
}

View File

@@ -506,6 +506,22 @@ bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vect
return true;
}
void b3RobotSimulatorClientAPI::setInternalSimFlags(int flags)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetInternalSimFlags(command, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
}
void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
{
if (!isConnected())
@@ -544,21 +560,70 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
}
void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
int b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
return -1;
}
b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_USER_CONSTRAINT_COMPLETED)
{
int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
return userConstraintUid;
}
}
return -1;
}
int b3RobotSimulatorClientAPI::changeConstraint(int constraintId, b3JointInfo* jointInfo)
{
if (!isConnected())
{
b3Warning("Not connected");
return -1;
}
b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
if (jointInfo->m_flags & eJointChangeMaxForce)
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
}
if (jointInfo->m_flags & eJointChangeChildFramePosition)
{
b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
}
if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
{
b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
}
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
int statusType = b3GetStatusType(statusHandle);
return statusType;
}
void b3RobotSimulatorClientAPI::removeConstraint(int constraintId)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
int statusType = b3GetStatusType(statusHandle);
}
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{
if (!isConnected())
@@ -669,6 +734,21 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
void b3RobotSimulatorClientAPI::setContactBreakingThreshold(double threshold)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetContactBreakingThreshold(command,threshold);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
{
if (!isConnected())
@@ -799,7 +879,7 @@ void b3RobotSimulatorClientAPI::configureDebugVisualizer(b3ConfigureDebugVisuali
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
{
vrEventsData->m_numControllerEvents = 0;
vrEventsData->m_controllerEvents = 0;
@@ -810,6 +890,7 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
}
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
}
@@ -831,6 +912,11 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
{
if (!isConnected())
{
b3Warning("Not connected");
return -1;
}
int loggingUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
@@ -861,6 +947,12 @@ int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType,
void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
@@ -870,6 +962,12 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
if (commandHandle)
{
@@ -882,3 +980,19 @@ void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
}
void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
if (durationInMicroSeconds>=0)
{
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
}
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}

View File

@@ -17,6 +17,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs
bool m_useMultiBody;
int m_flags;
b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn)
: m_startPosition(startPos),
m_startOrientation(startOrn),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_flags(0)
{
}
b3RobotSimulatorLoadUrdfFileArgs()
: m_startPosition(b3MakeVector3(0, 0, 0)),
m_startOrientation(b3Quaternion(0, 0, 0, 1)),
@@ -123,6 +132,8 @@ class b3RobotSimulatorClientAPI
struct b3RobotSimulatorClientAPI_InternalData* m_data;
public:
b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI();
@@ -158,7 +169,11 @@ public:
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
int changeConstraint(int constraintId, b3JointInfo* jointInfo);
void removeConstraint(int constraintId);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
@@ -172,11 +187,14 @@ public:
void setRealTimeSimulation(bool enableRealTimeSimulation);
void setInternalSimFlags(int flags);
void setGravity(const b3Vector3& gravityAcceleration);
void setTimeStep(double timeStepInSeconds);
void setNumSimulationSubSteps(int numSubSteps);
void setNumSolverIterations(int numIterations);
void setContactBreakingThreshold(double threshold);
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
@@ -190,8 +208,11 @@ public:
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds=b3AlignedObjectArray<int>(), int maxLogDof = -1);
void stopStateLogging(int stateLoggerUniqueId);
void getVREvents(b3VREventsData* vrEventsData);
void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

View File

@@ -1,3 +1,67 @@
myfiles =
{
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
"../../examples/SharedMemory/PhysicsDirect.cpp",
"../../examples/SharedMemory/PhysicsDirect.h",
"../../examples/SharedMemory/PhysicsDirectC_API.cpp",
"../../examples/SharedMemory/PhysicsDirectC_API.h",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h",
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
"../../examples/SharedMemory/PhysicsClientC_API.h",
"../../examples/SharedMemory/Win32SharedMemory.cpp",
"../../examples/SharedMemory/Win32SharedMemory.h",
"../../examples/SharedMemory/PosixSharedMemory.cpp",
"../../examples/SharedMemory/PosixSharedMemory.h",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
"../../examples/TinyRenderer/geometry.cpp",
"../../examples/TinyRenderer/model.cpp",
"../../examples/TinyRenderer/tgaimage.cpp",
"../../examples/TinyRenderer/our_gl.cpp",
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
"../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
}
project ("App_RobotSimulator")
language "C++"
@@ -87,72 +151,132 @@ if not _OPTIONS["no-enet"] then
defines {"BT_ENABLE_CLSOCKET"}
end
if _OPTIONS["audio"] then
files {
"../TinyAudio/b3ADSR.cpp",
"../TinyAudio/b3AudioListener.cpp",
"../TinyAudio/b3ReadWavFile.cpp",
"../TinyAudio/b3SoundEngine.cpp",
"../TinyAudio/b3SoundSource.cpp",
"../TinyAudio/b3WriteWavFile.cpp",
"../TinyAudio/RtAudio.cpp",
}
defines {"B3_ENABLE_TINY_AUDIO"}
if _OPTIONS["serial"] then
defines{"B3_ENABLE_SERIAL"}
includedirs {"../../examples/ThirdPartyLibs/serial/include"}
links {"serial"}
end
if os.is("Windows") then
links {"winmm","Wsock32","dsound"}
defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"}
end
if os.is("Linux") then initX11()
defines {"__OS_LINUX__","__LINUX_ALSA__"}
links {"asound","pthread"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
defines {"__OS_MACOSX__","__MACOSX_CORE__"}
end
end
files {
"RobotSimulatorMain.cpp",
"b3RobotSimulatorClientAPI.cpp",
"b3RobotSimulatorClientAPI.h",
"MinitaurSetup.cpp",
"MinitaurSetup.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
"../../examples/SharedMemory/PhysicsDirect.cpp",
"../../examples/SharedMemory/PhysicsDirect.h",
"../../examples/SharedMemory/PhysicsDirectC_API.cpp",
"../../examples/SharedMemory/PhysicsDirectC_API.h",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h",
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
"../../examples/SharedMemory/PhysicsClientC_API.h",
"../../examples/SharedMemory/Win32SharedMemory.cpp",
"../../examples/SharedMemory/Win32SharedMemory.h",
"../../examples/SharedMemory/PosixSharedMemory.cpp",
"../../examples/SharedMemory/PosixSharedMemory.h",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
"../../examples/TinyRenderer/geometry.cpp",
"../../examples/TinyRenderer/model.cpp",
"../../examples/TinyRenderer/tgaimage.cpp",
"../../examples/TinyRenderer/our_gl.cpp",
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
"../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
}
myfiles
}
if os.is("Linux") then
initX11()
end
project ("App_VRGloveHandSimulator")
language "C++"
kind "ConsoleApp"
includedirs {"../../src", "../../examples",
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"}
initOpenGL()
initGlew()
includedirs {
".",
"../../src",
"../ThirdPartyLibs",
}
if os.is("MacOSX") then
links{"Cocoa.framework"}
end
if (hasCL) then
links {
"Bullet3OpenCL_clew",
"Bullet3Dynamics",
"Bullet3Collision",
"Bullet3Geometry",
"Bullet3Common",
}
end
if _OPTIONS["audio"] then
files {
"../TinyAudio/b3ADSR.cpp",
"../TinyAudio/b3AudioListener.cpp",
"../TinyAudio/b3ReadWavFile.cpp",
"../TinyAudio/b3SoundEngine.cpp",
"../TinyAudio/b3SoundSource.cpp",
"../TinyAudio/b3WriteWavFile.cpp",
"../TinyAudio/RtAudio.cpp",
}
defines {"B3_ENABLE_TINY_AUDIO"}
if _OPTIONS["serial"] then
defines{"B3_ENABLE_SERIAL"}
includedirs {"../../examples/ThirdPartyLibs/serial/include"}
links {"serial"}
end
if os.is("Windows") then
links {"winmm","Wsock32","dsound"}
defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"}
end
if os.is("Linux") then initX11()
defines {"__OS_LINUX__","__LINUX_ALSA__"}
links {"asound","pthread"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
defines {"__OS_MACOSX__","__MACOSX_CORE__"}
end
end
files {
"VRGloveSimulatorMain.cpp",
"b3RobotSimulatorClientAPI.cpp",
"b3RobotSimulatorClientAPI.h",
myfiles
}
if os.is("Linux") then
initX11()
end

View File

@@ -50,6 +50,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
eRobotSimGUIHelperRemoveAllGraphicsInstances,
eRobotSimGUIHelperCopyCameraImageData,
eRobotSimGUIHelperRemoveGraphicsInstance,
};
#include <stdio.h>
@@ -320,6 +321,17 @@ public:
{
}
}
int m_graphicsInstanceRemove;
virtual void removeGraphicsInstance(int instanceUid)
{
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveGraphicsInstance);
m_graphicsInstanceRemove = instanceUid;
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
@@ -639,6 +651,14 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRemoveGraphicsInstance:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_data->m_multiThreadedHelper->m_graphicsInstanceRemove);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperCopyCameraImageData:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix,

View File

@@ -249,6 +249,8 @@ LINK_LIBRARIES(
../ThirdPartyLibs/openvr/samples/shared/Matrices.h
../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp
../ThirdPartyLibs/openvr/samples/shared/pathtools.h
../ThirdPartyLibs/openvr/samples/shared/strtools.cpp
../ThirdPartyLibs/openvr/samples/shared/strtools.h
../ThirdPartyLibs/openvr/samples/shared/Vectors.h
../MultiThreading/b3Win32ThreadSupport.cpp
../MultiThreading/b3Win32ThreadSupport.h
@@ -261,7 +263,7 @@ IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
ADD_CUSTOM_COMMAND(
TARGET App_PhysicsServer_SharedMemory_VR
POST_BUILD
COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/win64/openvr_api.dll ${CMAKE_CURRENT_BINARY_DIR}/openvr_api.dll
COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/win64/openvr_api.dll ${CMAKE_CURRENT_BINARY_DIR}/openvr64pi.dll
)
ELSE(CMAKE_CL_64)
ADD_CUSTOM_COMMAND(

View File

@@ -37,6 +37,8 @@ public:
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual int getUserConstraintId(int serialIndex) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

View File

@@ -160,6 +160,16 @@ b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClie
return 0;
}
void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_MJCF);
if (command->m_type == CMD_LOAD_MJCF)
{
command->m_mjcfArguments.m_flags = flags;
command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS;
}
}
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
{
@@ -1179,6 +1189,13 @@ int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniq
return 0;
}
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getUserConstraintId(serialIndex);
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{
@@ -1201,14 +1218,73 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
return cl->getNumJoints(bodyId);
}
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getJointInfo(bodyIndex, jointIndex, *info);
}
b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_GET_DYNAMICS_INFO;
command->m_getDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_getDynamicsInfoArgs.m_linkIndex = linkIndex;
return (b3SharedMemoryCommandHandle) command;
}
int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo;
btAssert(status->m_type == CMD_GET_DYNAMICS_INFO);
if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
return false;
info->m_mass = dynamicsInfo.m_mass;
info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
return true;
}
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CHANGE_DYNAMICS_INFO;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
b3Assert(mass > 0);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_mass = mass;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MASS;
return 0;
}
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_lateralFriction = lateralFriction;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION;
return 0;
}
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{
@@ -1308,6 +1384,24 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa
command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REMOVE_BODY;
command->m_updateFlags = BODY_DELETE_FLAG;
command->m_removeObjectArgs.m_numBodies = 1;
command->m_removeObjectArgs.m_bodyUniqueIds[0] = bodyUniqueId;
command->m_removeObjectArgs.m_numUserConstraints = 0;
return (b3SharedMemoryCommandHandle)command;
}
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -2636,6 +2730,41 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard
}
b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
int len = strlen(name);
if (len>=0 && len < (MAX_FILENAME_LENGTH+1))
{
command->m_type = CMD_PROFILE_TIMING;
strcpy(command->m_profile.m_name,name);
command->m_profile.m_name[len]=0;
} else
{
const char* invalid = "InvalidProfileTimingName";
int len = strlen(invalid);
strcpy(command->m_profile.m_name,invalid);
command->m_profile.m_name[len] = 0;
}
command->m_profile.m_durationInMicroSeconds = 0;
return (b3SharedMemoryCommandHandle)command;
}
void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_PROFILE_TIMING);
if (command->m_type == CMD_PROFILE_TIMING)
{
command->m_profile.m_durationInMicroSeconds = duration;
}
}
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
{
@@ -2782,7 +2911,6 @@ int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle,
return 0;
}
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

View File

@@ -60,6 +60,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
@@ -74,6 +76,14 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
@@ -90,6 +100,8 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
@@ -225,7 +237,7 @@ int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int fla
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
@@ -381,6 +393,8 @@ int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);

View File

@@ -53,6 +53,9 @@ protected:
int m_canvasRGBIndex;
int m_canvasDepthIndex;
int m_canvasSegMaskIndex;
btScalar m_lightPos[3];
btScalar m_specularCoeff;
void createButton(const char* name, int id, bool isTrigger );
@@ -278,6 +281,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
float lightPos[3];
lightPos[0] = m_lightPos[0];
lightPos[1] = m_lightPos[1];
lightPos[2] = m_lightPos[2];
b3RequestCameraImageSetLightDirection(commandHandle, lightPos);
b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
@@ -522,6 +531,7 @@ m_canvas(0),
m_canvasRGBIndex(-1),
m_canvasDepthIndex(-1),
m_canvasSegMaskIndex(-1),
m_specularCoeff(1.0),
m_numMotors(0),
m_options(options),
m_isOptionalServerConnected(false)
@@ -660,6 +670,29 @@ void PhysicsClientExample::createButtons()
}
}
}
{
SliderParams sliderLightPosX("light source position x",&m_lightPos[0]);
SliderParams sliderLightPosY("light source position y",&m_lightPos[1]);
SliderParams sliderLightPosZ("light source position z",&m_lightPos[2]);
SliderParams sliderSpecularCoeff("specular coefficient",&m_specularCoeff);
sliderLightPosX.m_minVal=-1.5;
sliderLightPosX.m_maxVal=1.5;
sliderLightPosY.m_minVal=-1.5;
sliderLightPosY.m_maxVal=1.5;
sliderLightPosZ.m_minVal=-1.5;
sliderLightPosZ.m_maxVal=1.5;
sliderSpecularCoeff.m_minVal=0;
sliderSpecularCoeff.m_maxVal=5.0;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosX);
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosY);
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosZ);
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderSpecularCoeff);
}
}
}
}
@@ -684,6 +717,10 @@ void PhysicsClientExample::initPhysics()
m_selectedBody = -1;
m_prevSelectedBody = -1;
m_lightPos[0] = 1.0;
m_lightPos[1] = 1.0;
m_lightPos[2] = 1.0;
{
m_canvas = m_guiHelper->get2dCanvasInterface();

View File

@@ -163,6 +163,15 @@ int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, str
return 0;
}
int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
{
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{
@@ -190,6 +199,28 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
delete m_data;
}
void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++)
{
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId);
}
}
void PhysicsClientSharedMemory::resetData()
{
m_data->m_debugLinesFrom.clear();
@@ -999,7 +1030,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_REMOVE_BODY_COMPLETED:
{
break;
}
case CMD_REMOVE_BODY_FAILED:
{
b3Warning("Removing body failed");
break;
}
case CMD_GET_DYNAMICS_INFO_COMPLETED:
{
break;
}
case CMD_GET_DYNAMICS_INFO_FAILED:
{
b3Warning("Request dynamics info failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@@ -1050,7 +1098,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
return 0;
}
}
if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED)
{
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
{
int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
removeCachedBody(bodyUniqueId);
}
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numUserConstraints;i++)
{
int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i];
m_data->m_userConstraintInfoMap.remove(key);
}
}
if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
{
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");

View File

@@ -13,6 +13,7 @@ protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
PhysicsClientSharedMemory();
@@ -47,6 +48,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
virtual void setSharedMemoryKey(int key);

View File

@@ -724,6 +724,26 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_REMOVE_BODY_FAILED:
{
b3Warning("Remove body failed\n");
break;
}
case CMD_REMOVE_BODY_COMPLETED:
{
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
{
int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
removeCachedBody(bodyUniqueId);
}
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numUserConstraints;i++)
{
int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i];
m_data->m_userConstraintInfoMap.remove(key);
}
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
@@ -909,6 +929,28 @@ int PhysicsDirect::getNumBodies() const
return m_data->m_bodyJointMap.size();
}
void PhysicsDirect::removeCachedBody(int bodyUniqueId)
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId);
}
}
int PhysicsDirect::getNumUserConstraints() const
{
return m_data->m_userConstraintInfoMap.size();
@@ -925,7 +967,14 @@ int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserCo
return 0;
}
int PhysicsDirect::getUserConstraintId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
{
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{

View File

@@ -29,6 +29,9 @@ protected:
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
@@ -66,6 +69,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

View File

@@ -108,6 +108,11 @@ int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3User
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
}
int PhysicsLoopBack::getUserConstraintId(int serialIndex) const
{
return m_data->m_physicsClient->getUserConstraintId(serialIndex);
}
///todo: move this out of the interface
void PhysicsLoopBack::setSharedMemoryKey(int key)
{

View File

@@ -51,6 +51,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

File diff suppressed because it is too large Load Diff

View File

@@ -95,6 +95,7 @@ public:
//logging of object states (position etc)
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);
void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
void enableRealTimeSimulation(bool enableRealTimeSim);

View File

@@ -179,6 +179,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIUserDebugRemoveItem,
eGUIUserDebugRemoveAllItems,
eGUIDumpFramesToVideo,
eGUIHelperRemoveGraphicsInstance,
};
@@ -778,6 +779,15 @@ public:
workerThreadWait();
}
int m_graphicsInstanceRemove;
virtual void removeGraphicsInstance(int graphicsUid)
{
m_graphicsInstanceRemove = graphicsUid;
m_cs->lock();
m_cs->setSharedParam(1,eGUIHelperRemoveGraphicsInstance);
workerThreadWait();
}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
@@ -1193,39 +1203,45 @@ public:
printf("key[%d]=%d state = %d\n",i,m_args[0].m_keyboardEvents[i].m_keyCode,m_args[0].m_keyboardEvents[i].m_keyState);
}
*/
double shift=0.1;
CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
if (window->isModifierKeyPressed(B3G_SHIFT))
shift=0.01;
if (key=='w' && state)
{
gVRTeleportPos1[0]+=0.1;
gVRTeleportPos1[0]+=shift;
saveCurrentSettingsVR();
}
if (key=='s' && state)
{
gVRTeleportPos1[0]-=0.1;
gVRTeleportPos1[0]-=shift;
saveCurrentSettingsVR();
}
if (key=='a' && state)
{
gVRTeleportPos1[1]-=0.1;
gVRTeleportPos1[1]-=shift;
saveCurrentSettingsVR();
}
if (key=='d' && state)
{
gVRTeleportPos1[1]+=0.1;
gVRTeleportPos1[1]+=shift;
saveCurrentSettingsVR();
}
if (key=='q' && state)
{
gVRTeleportPos1[2]+=0.1;
gVRTeleportPos1[2]+=shift;
saveCurrentSettingsVR();
}
if (key=='e' && state)
{
gVRTeleportPos1[2]-=0.1;
gVRTeleportPos1[2]-=shift;
saveCurrentSettingsVR();
}
if (key=='z' && state)
{
gVRTeleportRotZ+=0.1;
gVRTeleportRotZ+=shift;
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
}
@@ -1551,7 +1567,15 @@ void PhysicsServerExample::updateGraphics()
break;
}
case eGUIHelperRemoveGraphicsInstance:
{
m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_multiThreadedHelper->m_graphicsInstanceRemove);
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperCopyCameraImageData:
{
m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix,

View File

@@ -252,7 +252,7 @@ void PhysicsServerSharedMemory::processClientCommands()
if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
{
BT_PROFILE("processClientCommand");
//BT_PROFILE("processClientCommand");
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1);

View File

@@ -93,10 +93,13 @@ struct UrdfArgs
int m_urdfFlags;
};
struct MjcfArgs
{
char m_mjcfFileName[MAX_URDF_FILENAME_LENGTH];
int m_useMultiBody;
int m_flags;
};
struct BulletDataStreamArgs
@@ -106,6 +109,28 @@ struct BulletDataStreamArgs
char m_bodyName[MAX_FILENAME_LENGTH];
};
enum EnumChangeDynamicsInfoFlags
{
CHANGE_DYNAMICS_INFO_SET_MASS=1,
CHANGE_DYNAMICS_INFO_SET_COM=2,
CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION=4,
};
struct ChangeDynamicsInfoArgs
{
int m_bodyUniqueId;
int m_linkIndex;
double m_mass;
double m_COM[3];
double m_lateralFriction;
};
struct GetDynamicsInfoArgs
{
int m_bodyUniqueId;
int m_linkIndex;
};
struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
@@ -435,6 +460,22 @@ struct CreateBoxShapeArgs
double m_colorRGBA[4];
};
struct b3ObjectArgs
{
int m_numBodies;
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_numUserConstraints;
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
};
struct b3Profile
{
char m_name[MAX_FILENAME_LENGTH];
int m_durationInMicroSeconds;
};
struct SdfLoadedArgs
{
int m_numBodies;
@@ -556,6 +597,10 @@ enum EnumUserConstraintFlags
};
enum EnumBodyChangeFlags
{
BODY_DELETE_FLAG=1,
};
@@ -702,6 +747,8 @@ struct SharedMemoryCommand
struct MjcfArgs m_mjcfArguments;
struct FileArgs m_fileArguments;
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
struct ChangeDynamicsInfoArgs m_changeDynamicsInfoArgs;
struct GetDynamicsInfoArgs m_getDynamicsInfoArgs;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
@@ -728,6 +775,9 @@ struct SharedMemoryCommand
struct VRCameraState m_vrCameraStateArguments;
struct StateLoggingRequest m_stateLoggingArguments;
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
struct b3ObjectArgs m_removeObjectArgs;
struct b3Profile m_profile;
};
};
@@ -791,6 +841,8 @@ struct SharedMemoryStatus
struct SendRaycastHits m_raycastHits;
struct StateLoggingResultArgs m_stateLoggingResultArgs;
struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs;
struct b3ObjectArgs m_removeObjectArgs;
struct b3DynamicsInfo m_dynamicsInfo;
};
};

View File

@@ -55,6 +55,10 @@ enum EnumSharedMemoryClientCommand
CMD_CONFIGURE_OPENGL_VISUALIZER,
CMD_REQUEST_KEYBOARD_EVENTS_DATA,
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA,
CMD_REMOVE_BODY,
CMD_CHANGE_DYNAMICS_INFO,
CMD_GET_DYNAMICS_INFO,
CMD_PROFILE_TIMING,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -136,6 +140,10 @@ enum EnumSharedMemoryServerStatus
CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED,
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED,
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED,
CMD_REMOVE_BODY_COMPLETED,
CMD_REMOVE_BODY_FAILED,
CMD_GET_DYNAMICS_INFO_COMPLETED,
CMD_GET_DYNAMICS_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -167,6 +175,14 @@ enum JointType {
ePoint2PointType = 5,
};
enum b3JointInfoFlags
{
eJointChangeMaxForce = 1,
eJointChangeChildFramePosition = 2,
eJointChangeChildFrameOrientation = 4,
};
struct b3JointInfo
{
char* m_linkName;
@@ -207,6 +223,12 @@ struct b3BodyInfo
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
};
struct b3DynamicsInfo
{
double m_mass;
double m_localInertialPosition[3];
double m_lateralFrictionCoeff;
};
// copied from btMultiBodyLink.h
enum SensorType {
@@ -317,11 +339,7 @@ struct b3VREventsData
{
int m_numControllerEvents;
struct b3VRControllerEvent* m_controllerEvents;
int m_numHmdEvents;
struct b3VRMoveEvent* m_hmdEvents;
int m_numGenericTrackerEvents;
struct b3VRMoveEvent* m_genericTrackerEvents;
};
@@ -375,6 +393,7 @@ enum b3StateLoggingType
STATE_LOGGING_VIDEO_MP4 = 3,
STATE_LOGGING_COMMANDS = 4,
STATE_LOGGING_CONTACT_POINTS = 5,
STATE_LOGGING_PROFILE_TIMINGS = 6,
};
@@ -490,6 +509,8 @@ enum eURDF_Flags
{
URDF_USE_INERTIA_FROM_FILE=2,//sync with URDF2Bullet.h 'ConvertURDFFlags'
URDF_USE_SELF_COLLISION=8,//see CUF_USE_SELF_COLLISION
URDF_USE_SELF_COLLISION_EXCLUDE_PARENT=16,
URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
};
#endif//SHARED_MEMORY_PUBLIC_H

View File

@@ -526,7 +526,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
}
else
{
// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
// We have to see something, take collision shape. Useful for MuJoCo xml, where there are no explicit visual shapes.
useVisual = false;
cnt = linkPtr->m_collisionArray.size();
}

View File

@@ -282,7 +282,36 @@ if os.is("Windows") then
end
end
if _OPTIONS["audio"] then
files {
"../TinyAudio/b3ADSR.cpp",
"../TinyAudio/b3AudioListener.cpp",
"../TinyAudio/b3ReadWavFile.cpp",
"../TinyAudio/b3SoundEngine.cpp",
"../TinyAudio/b3SoundSource.cpp",
"../TinyAudio/b3WriteWavFile.cpp",
"../TinyAudio/RtAudio.cpp",
}
defines {"B3_ENABLE_TINY_AUDIO"}
if os.is("Windows") then
links {"winmm","Wsock32","dsound"}
defines {"WIN32","__WINDOWS_MM__","__WINDOWS_DS__"}
end
if os.is("Linux") then initX11()
defines {"__OS_LINUX__","__LINUX_ALSA__"}
links {"asound","pthread"}
end
if os.is("MacOSX") then
links{"Cocoa.framework"}
links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
defines {"__OS_MACOSX__","__MACOSX_CORE__"}
end
end
includedirs {
".","../../src", "../ThirdPartyLibs",
"../ThirdPartyLibs/openvr/headers",
@@ -315,6 +344,7 @@ if os.is("Windows") then
"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
"../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
"../ThirdPartyLibs/openvr/samples/shared/strtools.cpp",
"../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
"../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
"../ThirdPartyLibs/openvr/samples/shared/Vectors.h",

View File

@@ -113,6 +113,8 @@ myfiles =
"../../Importers/ImportURDFDemo/URDF2Bullet.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3Clock.cpp",
"../../Utils/ChromeTraceUtil.cpp",
"../../Utils/ChromeTraceUtil.h",
"../../Utils/RobotLoggingUtil.cpp",
"../../Utils/RobotLoggingUtil.h",
"../../../Extras/Serialize/BulletWorldImporter/*",

View File

@@ -104,6 +104,8 @@ myfiles =
"../../Importers/ImportURDFDemo/URDF2Bullet.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3Clock.cpp",
"../../Utils/ChromeTraceUtil.cpp",
"../../Utils/ChromeTraceUtil.h",
"../../Utils/RobotLoggingUtil.cpp",
"../../Utils/RobotLoggingUtil.h",
"../../../Extras/Serialize/BulletWorldImporter/*",

View File

@@ -355,7 +355,7 @@ void MyKeyboardCallback(int key, int state)
}
else
{
b3ChromeUtilsStopTimingsAndWriteJsonFile();
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
}
}
if (sExample)
@@ -2342,7 +2342,7 @@ int main(int argc, char *argv[])
if (args.CheckCmdLineFlag("tracing"))
{
b3ChromeUtilsStopTimingsAndWriteJsonFile();
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
}

View File

@@ -21,19 +21,13 @@ struct VkPhysicalDevice_T;
struct VkInstance_T;
struct VkQueue_T;
// Forward declarations to avoid requiring d3d12.h
struct ID3D12Resource;
struct ID3D12CommandQueue;
namespace vr
{
#if defined(__linux__) || defined(__APPLE__)
// The 32-bit version of gcc has the alignment requirement for uint64 and double set to
// 4 meaning that even with #pragma pack(8) these types will only be four-byte aligned.
// The 64-bit version of gcc has the alignment requirement for these types set to
// 8 meaning that unless we use #pragma pack(4) our structures will get bigger.
// The 64-bit structure packing has to match the 32-bit structure packing for each platform.
#pragma pack( push, 4 )
#else
#pragma pack( push, 8 )
#endif
#pragma pack( push, 8 )
typedef void* glSharedTextureHandle_t;
typedef int32_t glInt_t;
@@ -116,6 +110,8 @@ enum ETextureType
TextureType_DirectX = 0, // Handle is an ID3D11Texture
TextureType_OpenGL = 1, // Handle is an OpenGL texture name or an OpenGL render buffer name, depending on submit flags
TextureType_Vulkan = 2, // Handle is a pointer to a VRVulkanTextureData_t structure
TextureType_IOSurface = 3, // Handle is a macOS cross-process-sharable IOSurfaceRef
TextureType_DirectX12 = 4, // Handle is a pointer to a D3D12TextureData_t structure
};
enum EColorSpace
@@ -164,6 +160,7 @@ enum ETrackedDeviceClass
TrackedDeviceClass_Controller = 2, // Tracked controllers
TrackedDeviceClass_GenericTracker = 3, // Generic trackers, similar to controllers
TrackedDeviceClass_TrackingReference = 4, // Camera and base stations that serve as tracking reference points
TrackedDeviceClass_DisplayRedirect = 5, // Accessories that aren't necessarily tracked themselves, but may redirect video output from other tracked devices
};
@@ -199,6 +196,30 @@ enum ETrackingUniverseOrigin
TrackingUniverseRawAndUncalibrated = 2, // Poses are provided in the coordinate system defined by the driver. It has Y up and is unified for devices of the same driver. You usually don't want this one.
};
// Refers to a single container of properties
typedef uint64_t PropertyContainerHandle_t;
typedef uint32_t PropertyTypeTag_t;
static const PropertyContainerHandle_t k_ulInvalidPropertyContainer = 0;
static const PropertyTypeTag_t k_unInvalidPropertyTag = 0;
// Use these tags to set/get common types as struct properties
static const PropertyTypeTag_t k_unFloatPropertyTag = 1;
static const PropertyTypeTag_t k_unInt32PropertyTag = 2;
static const PropertyTypeTag_t k_unUint64PropertyTag = 3;
static const PropertyTypeTag_t k_unBoolPropertyTag = 4;
static const PropertyTypeTag_t k_unStringPropertyTag = 5;
static const PropertyTypeTag_t k_unHmdMatrix34PropertyTag = 20;
static const PropertyTypeTag_t k_unHmdMatrix44PropertyTag = 21;
static const PropertyTypeTag_t k_unHmdVector3PropertyTag = 22;
static const PropertyTypeTag_t k_unHmdVector4PropertyTag = 23;
static const PropertyTypeTag_t k_unHiddenAreaPropertyTag = 30;
static const PropertyTypeTag_t k_unOpenVRInternalReserved_Start = 1000;
static const PropertyTypeTag_t k_unOpenVRInternalReserved_End = 10000;
/** Each entry in this enum represents a property that can be retrieved about a
* tracked device. Many fields are only valid for one ETrackedDeviceClass. */
@@ -241,6 +262,8 @@ enum ETrackedDeviceProperty
Prop_DriverVersion_String = 1031,
Prop_Firmware_ForceUpdateRequired_Bool = 1032,
Prop_ViveSystemButtonFixRequired_Bool = 1033,
Prop_ParentDriver_Uint64 = 1034,
Prop_ResourceRoot_String = 1035,
// Properties that are unique to TrackedDeviceClass_HMD
Prop_ReportsTimeSinceVSync_Bool = 2000,
@@ -281,6 +304,11 @@ enum ETrackedDeviceProperty
Prop_ScreenshotVerticalFieldOfViewDegrees_Float = 2035,
Prop_DisplaySuppressed_Bool = 2036,
Prop_DisplayAllowNightMode_Bool = 2037,
Prop_DisplayMCImageWidth_Int32 = 2038,
Prop_DisplayMCImageHeight_Int32 = 2039,
Prop_DisplayMCImageNumChannels_Int32 = 2040,
Prop_DisplayMCImageData_Binary = 2041,
Prop_SecondsFromPhotonsToVblank_Float = 2042,
// Properties that are unique to TrackedDeviceClass_Controller
Prop_AttachedDeviceId_String = 3000,
@@ -312,6 +340,19 @@ enum ETrackedDeviceProperty
Prop_NamedIconPathDeviceStandby_String = 5007, // PNG for static icon, or GIF for animation, 50x32 for headsets and 32x32 for others
Prop_NamedIconPathDeviceAlertLow_String = 5008, // PNG for static icon, or GIF for animation, 50x32 for headsets and 32x32 for others
// Properties that are used by helpers, but are opaque to applications
Prop_DisplayHiddenArea_Binary_Start = 5100,
Prop_DisplayHiddenArea_Binary_End = 5150,
// Properties that are unique to drivers
Prop_UserConfigPath_String = 6000,
Prop_InstallPath_String = 6001,
Prop_HasDisplayComponent_Bool = 6002,
Prop_HasControllerComponent_Bool = 6003,
Prop_HasCameraComponent_Bool = 6004,
Prop_HasDriverDirectModeComponent_Bool = 6005,
Prop_HasVirtualDisplayComponent_Bool = 6006,
// Vendors are free to expose private debug data in this reserved region
Prop_VendorSpecific_Reserved_Start = 10000,
Prop_VendorSpecific_Reserved_End = 10999,
@@ -327,13 +368,14 @@ enum ETrackedPropertyError
TrackedProp_WrongDataType = 1,
TrackedProp_WrongDeviceClass = 2,
TrackedProp_BufferTooSmall = 3,
TrackedProp_UnknownProperty = 4,
TrackedProp_UnknownProperty = 4, // Driver has not set the property (and may not ever).
TrackedProp_InvalidDevice = 5,
TrackedProp_CouldNotContactServer = 6,
TrackedProp_ValueNotProvidedByDevice = 7,
TrackedProp_StringExceedsMaximumLength = 8,
TrackedProp_NotYetAvailable = 9, // The property value isn't known yet, but is expected soon. Call again later.
TrackedProp_PermissionDenied = 10,
TrackedProp_InvalidOperation = 11,
};
/** Allows the application to control what part of the provided texture will be used in the
@@ -376,6 +418,14 @@ struct VRVulkanTextureData_t
uint32_t m_nWidth, m_nHeight, m_nFormat, m_nSampleCount;
};
/** Data required for passing D3D12 textures to IVRCompositor::Submit.
* Be sure to call OpenVR_Shutdown before destroying these resources. */
struct D3D12TextureData_t
{
ID3D12Resource *m_pResource;
ID3D12CommandQueue *m_pCommandQueue;
uint32_t m_nNodeMask;
};
/** Status of the overall system or tracked objects */
enum EVRState
@@ -399,7 +449,7 @@ enum EVREventType
VREvent_TrackedDeviceActivated = 100,
VREvent_TrackedDeviceDeactivated = 101,
VREvent_TrackedDeviceUpdated = 102,
VREvent_TrackedDeviceUserInteractionStarted = 103,
VREvent_TrackedDeviceUserInteractionStarted = 103,
VREvent_TrackedDeviceUserInteractionEnded = 104,
VREvent_IpdChanged = 105,
VREvent_EnterStandbyMode = 106,
@@ -407,6 +457,7 @@ enum EVREventType
VREvent_TrackedDeviceRoleChanged = 108,
VREvent_WatchdogWakeUpRequested = 109,
VREvent_LensDistortionChanged = 110,
VREvent_PropertyChanged = 111,
VREvent_ButtonPress = 200, // data is controller
VREvent_ButtonUnpress = 201, // data is controller
@@ -436,78 +487,83 @@ enum EVREventType
VREvent_OverlayShown = 500,
VREvent_OverlayHidden = 501,
VREvent_DashboardActivated = 502,
VREvent_DashboardDeactivated = 503,
VREvent_DashboardThumbSelected = 504, // Sent to the overlay manager - data is overlay
VREvent_DashboardRequested = 505, // Sent to the overlay manager - data is overlay
VREvent_ResetDashboard = 506, // Send to the overlay manager
VREvent_RenderToast = 507, // Send to the dashboard to render a toast - data is the notification ID
VREvent_ImageLoaded = 508, // Sent to overlays when a SetOverlayRaw or SetOverlayFromFile call finishes loading
VREvent_ShowKeyboard = 509, // Sent to keyboard renderer in the dashboard to invoke it
VREvent_HideKeyboard = 510, // Sent to keyboard renderer in the dashboard to hide it
VREvent_OverlayGamepadFocusGained = 511, // Sent to an overlay when IVROverlay::SetFocusOverlay is called on it
VREvent_OverlayGamepadFocusLost = 512, // Send to an overlay when it previously had focus and IVROverlay::SetFocusOverlay is called on something else
VREvent_DashboardActivated = 502,
VREvent_DashboardDeactivated = 503,
VREvent_DashboardThumbSelected = 504, // Sent to the overlay manager - data is overlay
VREvent_DashboardRequested = 505, // Sent to the overlay manager - data is overlay
VREvent_ResetDashboard = 506, // Send to the overlay manager
VREvent_RenderToast = 507, // Send to the dashboard to render a toast - data is the notification ID
VREvent_ImageLoaded = 508, // Sent to overlays when a SetOverlayRaw or SetOverlayFromFile call finishes loading
VREvent_ShowKeyboard = 509, // Sent to keyboard renderer in the dashboard to invoke it
VREvent_HideKeyboard = 510, // Sent to keyboard renderer in the dashboard to hide it
VREvent_OverlayGamepadFocusGained = 511, // Sent to an overlay when IVROverlay::SetFocusOverlay is called on it
VREvent_OverlayGamepadFocusLost = 512, // Send to an overlay when it previously had focus and IVROverlay::SetFocusOverlay is called on something else
VREvent_OverlaySharedTextureChanged = 513,
VREvent_DashboardGuideButtonDown = 514,
VREvent_DashboardGuideButtonUp = 515,
VREvent_ScreenshotTriggered = 516, // Screenshot button combo was pressed, Dashboard should request a screenshot
VREvent_ImageFailed = 517, // Sent to overlays when a SetOverlayRaw or SetOverlayfromFail fails to load
VREvent_DashboardOverlayCreated = 518,
VREvent_DashboardGuideButtonDown = 514,
VREvent_DashboardGuideButtonUp = 515,
VREvent_ScreenshotTriggered = 516, // Screenshot button combo was pressed, Dashboard should request a screenshot
VREvent_ImageFailed = 517, // Sent to overlays when a SetOverlayRaw or SetOverlayfromFail fails to load
VREvent_DashboardOverlayCreated = 518,
// Screenshot API
VREvent_RequestScreenshot = 520, // Sent by vrclient application to compositor to take a screenshot
VREvent_ScreenshotTaken = 521, // Sent by compositor to the application that the screenshot has been taken
VREvent_ScreenshotFailed = 522, // Sent by compositor to the application that the screenshot failed to be taken
VREvent_SubmitScreenshotToDashboard = 523, // Sent by compositor to the dashboard that a completed screenshot was submitted
VREvent_ScreenshotProgressToDashboard = 524, // Sent by compositor to the dashboard that a completed screenshot was submitted
VREvent_RequestScreenshot = 520, // Sent by vrclient application to compositor to take a screenshot
VREvent_ScreenshotTaken = 521, // Sent by compositor to the application that the screenshot has been taken
VREvent_ScreenshotFailed = 522, // Sent by compositor to the application that the screenshot failed to be taken
VREvent_SubmitScreenshotToDashboard = 523, // Sent by compositor to the dashboard that a completed screenshot was submitted
VREvent_ScreenshotProgressToDashboard = 524, // Sent by compositor to the dashboard that a completed screenshot was submitted
VREvent_PrimaryDashboardDeviceChanged = 525,
VREvent_Notification_Shown = 600,
VREvent_Notification_Hidden = 601,
VREvent_Notification_BeginInteraction = 602,
VREvent_Notification_Destroyed = 603,
VREvent_Quit = 700, // data is process
VREvent_ProcessQuit = 701, // data is process
VREvent_QuitAborted_UserPrompt = 702, // data is process
VREvent_QuitAcknowledged = 703, // data is process
VREvent_DriverRequestedQuit = 704, // The driver has requested that SteamVR shut down
VREvent_Quit = 700, // data is process
VREvent_ProcessQuit = 701, // data is process
VREvent_QuitAborted_UserPrompt = 702, // data is process
VREvent_QuitAcknowledged = 703, // data is process
VREvent_DriverRequestedQuit = 704, // The driver has requested that SteamVR shut down
VREvent_ChaperoneDataHasChanged = 800,
VREvent_ChaperoneUniverseHasChanged = 801,
VREvent_ChaperoneTempDataHasChanged = 802,
VREvent_ChaperoneSettingsHaveChanged = 803,
VREvent_SeatedZeroPoseReset = 804,
VREvent_ChaperoneDataHasChanged = 800,
VREvent_ChaperoneUniverseHasChanged = 801,
VREvent_ChaperoneTempDataHasChanged = 802,
VREvent_ChaperoneSettingsHaveChanged = 803,
VREvent_SeatedZeroPoseReset = 804,
VREvent_AudioSettingsHaveChanged = 820,
VREvent_AudioSettingsHaveChanged = 820,
VREvent_BackgroundSettingHasChanged = 850,
VREvent_CameraSettingsHaveChanged = 851,
VREvent_ReprojectionSettingHasChanged = 852,
VREvent_ModelSkinSettingsHaveChanged = 853,
VREvent_EnvironmentSettingsHaveChanged = 854,
VREvent_PowerSettingsHaveChanged = 855,
VREvent_BackgroundSettingHasChanged = 850,
VREvent_CameraSettingsHaveChanged = 851,
VREvent_ReprojectionSettingHasChanged = 852,
VREvent_ModelSkinSettingsHaveChanged = 853,
VREvent_EnvironmentSettingsHaveChanged = 854,
VREvent_PowerSettingsHaveChanged = 855,
VREvent_StatusUpdate = 900,
VREvent_StatusUpdate = 900,
VREvent_MCImageUpdated = 1000,
VREvent_MCImageUpdated = 1000,
VREvent_FirmwareUpdateStarted = 1100,
VREvent_FirmwareUpdateFinished = 1101,
VREvent_FirmwareUpdateStarted = 1100,
VREvent_FirmwareUpdateFinished = 1101,
VREvent_KeyboardClosed = 1200,
VREvent_KeyboardCharInput = 1201,
VREvent_KeyboardDone = 1202, // Sent when DONE button clicked on keyboard
VREvent_KeyboardClosed = 1200,
VREvent_KeyboardCharInput = 1201,
VREvent_KeyboardDone = 1202, // Sent when DONE button clicked on keyboard
VREvent_ApplicationTransitionStarted = 1300,
VREvent_ApplicationTransitionAborted = 1301,
VREvent_ApplicationTransitionNewAppStarted = 1302,
VREvent_ApplicationListUpdated = 1303,
VREvent_ApplicationMimeTypeLoad = 1304,
VREvent_ApplicationTransitionStarted = 1300,
VREvent_ApplicationTransitionAborted = 1301,
VREvent_ApplicationTransitionNewAppStarted = 1302,
VREvent_ApplicationListUpdated = 1303,
VREvent_ApplicationMimeTypeLoad = 1304,
VREvent_ApplicationTransitionNewAppLaunchComplete = 1305,
VREvent_ProcessConnected = 1306,
VREvent_ProcessDisconnected = 1307,
VREvent_Compositor_MirrorWindowShown = 1400,
VREvent_Compositor_MirrorWindowHidden = 1401,
VREvent_Compositor_ChaperoneBoundsShown = 1410,
VREvent_Compositor_ChaperoneBoundsHidden = 1411,
VREvent_Compositor_MirrorWindowShown = 1400,
VREvent_Compositor_MirrorWindowHidden = 1401,
VREvent_Compositor_ChaperoneBoundsShown = 1410,
VREvent_Compositor_ChaperoneBoundsHidden = 1411,
VREvent_TrackedCamera_StartVideoStream = 1500,
VREvent_TrackedCamera_StopVideoStream = 1501,
@@ -515,26 +571,30 @@ enum EVREventType
VREvent_TrackedCamera_ResumeVideoStream = 1503,
VREvent_TrackedCamera_EditingSurface = 1550,
VREvent_PerformanceTest_EnableCapture = 1600,
VREvent_PerformanceTest_DisableCapture = 1601,
VREvent_PerformanceTest_FidelityLevel = 1602,
VREvent_PerformanceTest_EnableCapture = 1600,
VREvent_PerformanceTest_DisableCapture = 1601,
VREvent_PerformanceTest_FidelityLevel = 1602,
VREvent_MessageOverlay_Closed = 1650,
VREvent_MessageOverlay_Closed = 1650,
// Vendors are free to expose private events in this reserved region
VREvent_VendorSpecific_Reserved_Start = 10000,
VREvent_VendorSpecific_Reserved_End = 19999,
VREvent_VendorSpecific_Reserved_Start = 10000,
VREvent_VendorSpecific_Reserved_End = 19999,
};
/** Level of Hmd activity */
// UserInteraction_Timeout means the device is in the process of timing out.
// InUse = ( k_EDeviceActivityLevel_UserInteraction || k_EDeviceActivityLevel_UserInteraction_Timeout )
// VREvent_TrackedDeviceUserInteractionStarted fires when the devices transitions from Standby -> UserInteraction or Idle -> UserInteraction.
// VREvent_TrackedDeviceUserInteractionEnded fires when the devices transitions from UserInteraction_Timeout -> Idle
enum EDeviceActivityLevel
{
k_EDeviceActivityLevel_Unknown = -1,
k_EDeviceActivityLevel_Idle = 0,
k_EDeviceActivityLevel_UserInteraction = 1,
k_EDeviceActivityLevel_UserInteraction_Timeout = 2,
k_EDeviceActivityLevel_Standby = 3,
{
k_EDeviceActivityLevel_Unknown = -1,
k_EDeviceActivityLevel_Idle = 0, // No activity for the last 10 seconds
k_EDeviceActivityLevel_UserInteraction = 1, // Activity (movement or prox sensor) is happening now
k_EDeviceActivityLevel_UserInteraction_Timeout = 2, // No activity for the last 0.5 seconds
k_EDeviceActivityLevel_Standby = 3, // Idle for at least 5 seconds (configurable in Settings -> Power Management)
};
@@ -711,6 +771,12 @@ struct VREvent_MessageOverlay_t
uint32_t unVRMessageOverlayResponse; // vr::VRMessageOverlayResponse enum
};
struct VREvent_Property_t
{
PropertyContainerHandle_t container;
ETrackedDeviceProperty prop;
};
/** NOTE!!! If you change this you MUST manually update openvr_interop.cs.py */
typedef union
{
@@ -733,8 +799,16 @@ typedef union
VREvent_ApplicationLaunch_t applicationLaunch;
VREvent_EditingCameraSurface_t cameraSurface;
VREvent_MessageOverlay_t messageOverlay;
VREvent_Property_t property;
} VREvent_Data_t;
#if defined(__linux__) || defined(__APPLE__)
// This structure was originally defined mis-packed on Linux, preserved for
// compatibility.
#pragma pack( push, 4 )
#endif
/** An event posted by the server to all running applications */
struct VREvent_t
{
@@ -745,6 +819,9 @@ struct VREvent_t
VREvent_Data_t data;
};
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( pop )
#endif
/** The mesh to draw into the stencil (or depth) buffer to perform
* early stencil (or depth) kills of pixels that will never appear on the HMD.
@@ -764,6 +841,8 @@ enum EHiddenAreaMeshType
k_eHiddenAreaMesh_Standard = 0,
k_eHiddenAreaMesh_Inverse = 1,
k_eHiddenAreaMesh_LineLoop = 2,
k_eHiddenAreaMesh_Max = 3,
};
@@ -791,6 +870,12 @@ struct VRControllerAxis_t
static const uint32_t k_unControllerStateAxisCount = 5;
#if defined(__linux__) || defined(__APPLE__)
// This structure was originally defined mis-packed on Linux, preserved for
// compatibility.
#pragma pack( push, 4 )
#endif
/** Holds all the state of a controller at one moment in time. */
struct VRControllerState001_t
{
@@ -805,6 +890,9 @@ struct VRControllerState001_t
// Axis data for the controller's analog inputs
VRControllerAxis_t rAxis[ k_unControllerStateAxisCount ];
};
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( pop )
#endif
typedef VRControllerState001_t VRControllerState_t;
@@ -887,6 +975,7 @@ enum EVRApplicationType
// interfaces (like IVRSettings and IVRApplications) but not hardware.
VRApplication_VRMonitor = 5, // Reserved for vrmonitor
VRApplication_SteamWatchdog = 6,// Reserved for Steam
VRApplication_Bootstrapper = 7, // Start up SteamVR
VRApplication_Max
};
@@ -1173,9 +1262,7 @@ public:
/** [D3D10/11 Only]
* Returns the adapter index that the user should pass into EnumAdapters to create the device
* and swap chain in DX10 and DX11. If an error occurs the index will be set to -1. The index will
* also be -1 if the headset is in direct mode on the driver side instead of using the compositor's
* builtin direct mode support.
* and swap chain in DX10 and DX11. If an error occurs the index will be set to -1.
*/
virtual void GetDXGIOutputInfo( int32_t *pnAdapterIndex ) = 0;
@@ -1453,6 +1540,7 @@ namespace vr
VRApplicationProperty_IsDashboardOverlay_Bool = 60,
VRApplicationProperty_IsTemplate_Bool = 61,
VRApplicationProperty_IsInstanced_Bool = 62,
VRApplicationProperty_IsInternal_Bool = 63,
VRApplicationProperty_LastLaunchTime_Uint64 = 70,
};
@@ -1600,6 +1688,11 @@ namespace vr
* If working directory is NULL or "" the directory portion of the binary path will be
* the working directory. */
virtual EVRApplicationError LaunchInternalProcess( const char *pchBinaryPath, const char *pchArguments, const char *pchWorkingDirectory ) = 0;
/** Returns the current scene process ID according to the application system. A scene process will get scene
* focus once it starts rendering, but it will appear here once it calls VR_Init with the Scene application
* type. */
virtual uint32_t GetCurrentSceneProcessId() = 0;
};
static const char * const IVRApplications_Version = "IVRApplications_006";
@@ -1657,7 +1750,6 @@ namespace vr
static const char * const k_pch_SteamVR_ForcedHmdKey_String = "forcedHmd";
static const char * const k_pch_SteamVR_DisplayDebug_Bool = "displayDebug";
static const char * const k_pch_SteamVR_DebugProcessPipe_String = "debugProcessPipe";
static const char * const k_pch_SteamVR_EnableDistortion_Bool = "enableDistortion";
static const char * const k_pch_SteamVR_DisplayDebugX_Int32 = "displayDebugX";
static const char * const k_pch_SteamVR_DisplayDebugY_Int32 = "displayDebugY";
static const char * const k_pch_SteamVR_SendSystemButtonToAllApps_Bool= "sendSystemButtonToAllApps";
@@ -1708,7 +1800,6 @@ namespace vr
//-----------------------------------------------------------------------------
// null keys
static const char * const k_pch_Null_Section = "driver_null";
static const char * const k_pch_Null_EnableNullDriver_Bool = "enable";
static const char * const k_pch_Null_SerialNumber_String = "serialNumber";
static const char * const k_pch_Null_ModelNumber_String = "modelNumber";
static const char * const k_pch_Null_WindowX_Int32 = "windowX";
@@ -1809,22 +1900,17 @@ namespace vr
// model skin keys
static const char * const k_pch_modelskin_Section = "modelskins";
//-----------------------------------------------------------------------------
// driver keys - These could be checked in any driver_<name> section
static const char * const k_pch_Driver_Enable_Bool = "enable";
} // namespace vr
// ivrchaperone.h
namespace vr
{
#if defined(__linux__) || defined(__APPLE__)
// The 32-bit version of gcc has the alignment requirement for uint64 and double set to
// 4 meaning that even with #pragma pack(8) these types will only be four-byte aligned.
// The 64-bit version of gcc has the alignment requirement for these types set to
// 8 meaning that unless we use #pragma pack(4) our structures will get bigger.
// The 64-bit structure packing has to match the 32-bit structure packing for each platform.
#pragma pack( push, 4 )
#else
#pragma pack( push, 8 )
#endif
#pragma pack( push, 8 )
enum ChaperoneCalibrationState
{
@@ -1985,16 +2071,7 @@ static const char * const IVRChaperoneSetup_Version = "IVRChaperoneSetup_005";
namespace vr
{
#if defined(__linux__) || defined(__APPLE__)
// The 32-bit version of gcc has the alignment requirement for uint64 and double set to
// 4 meaning that even with #pragma pack(8) these types will only be four-byte aligned.
// The 64-bit version of gcc has the alignment requirement for these types set to
// 8 meaning that unless we use #pragma pack(4) our structures will get bigger.
// The 64-bit structure packing has to match the 32-bit structure packing for each platform.
#pragma pack( push, 4 )
#else
#pragma pack( push, 8 )
#endif
#pragma pack( push, 8 )
/** Errors that can occur with the VR compositor */
enum EVRCompositorError
@@ -2237,8 +2314,10 @@ public:
/** Temporarily suspends rendering (useful for finer control over scene transitions). */
virtual void SuspendRendering( bool bSuspend ) = 0;
/** Opens a shared D3D11 texture with the undistorted composited image for each eye. */
/** Opens a shared D3D11 texture with the undistorted composited image for each eye. Use ReleaseMirrorTextureD3D11 when finished
* instead of calling Release on the resource itself. */
virtual vr::EVRCompositorError GetMirrorTextureD3D11( vr::EVREye eEye, void *pD3D11DeviceOrResource, void **ppD3D11ShaderResourceView ) = 0;
virtual void ReleaseMirrorTextureD3D11( void *pD3D11ShaderResourceView ) = 0;
/** Access to mirror textures from OpenGL. */
virtual vr::EVRCompositorError GetMirrorTextureGL( vr::EVREye eEye, vr::glUInt_t *pglTextureId, vr::glSharedTextureHandle_t *pglSharedTextureHandle ) = 0;
@@ -2258,7 +2337,7 @@ public:
};
static const char * const IVRCompositor_Version = "IVRCompositor_019";
static const char * const IVRCompositor_Version = "IVRCompositor_020";
} // namespace vr
@@ -2268,16 +2347,7 @@ static const char * const IVRCompositor_Version = "IVRCompositor_019";
namespace vr
{
#if defined(__linux__) || defined(__APPLE__)
// The 32-bit version of gcc has the alignment requirement for uint64 and double set to
// 4 meaning that even with #pragma pack(8) these types will only be four-byte aligned.
// The 64-bit version of gcc has the alignment requirement for these types set to
// 8 meaning that unless we use #pragma pack(4) our structures will get bigger.
// The 64-bit structure packing has to match the 32-bit structure packing for each platform.
#pragma pack( push, 4 )
#else
#pragma pack( push, 8 )
#endif
#pragma pack( push, 8 )
// Used for passing graphic data
struct NotificationBitmap_t
@@ -2537,7 +2607,7 @@ namespace vr
virtual EVROverlayError FindOverlay( const char *pchOverlayKey, VROverlayHandle_t * pOverlayHandle ) = 0;
/** Creates a new named overlay. All overlays start hidden and with default settings. */
virtual EVROverlayError CreateOverlay( const char *pchOverlayKey, const char *pchOverlayFriendlyName, VROverlayHandle_t * pOverlayHandle ) = 0;
virtual EVROverlayError CreateOverlay( const char *pchOverlayKey, const char *pchOverlayName, VROverlayHandle_t * pOverlayHandle ) = 0;
/** Destroys the specified overlay. When an application calls VR_Shutdown all overlays created by that app are
* automatically destroyed. */
@@ -2562,6 +2632,9 @@ namespace vr
* the terminating null character. k_unVROverlayMaxNameLength will be enough bytes to fit the string. */
virtual uint32_t GetOverlayName( VROverlayHandle_t ulOverlayHandle, VR_OUT_STRING() char *pchValue, uint32_t unBufferSize, EVROverlayError *pError = 0L ) = 0;
/** set the name to use for this overlay */
virtual EVROverlayError SetOverlayName( VROverlayHandle_t ulOverlayHandle, const char *pchName ) = 0;
/** Gets the raw image data from an overlay. Overlay image data is always returned as RGBA data, 4 bytes per pixel. If the buffer is not large enough, width and height
* will be set and VROverlayError_ArrayTooSmall is returned. */
virtual EVROverlayError GetOverlayImageData( VROverlayHandle_t ulOverlayHandle, void *pvBuffer, uint32_t unBufferSize, uint32_t *punWidth, uint32_t *punHeight ) = 0;
@@ -2570,7 +2643,6 @@ namespace vr
* of the error enum value for all valid error codes */
virtual const char *GetOverlayErrorNameFromEnum( EVROverlayError error ) = 0;
// ---------------------------------------------
// Overlay rendering methods
// ---------------------------------------------
@@ -2647,6 +2719,13 @@ namespace vr
/** Gets the part of the texture to use for the overlay. UV Min is the upper left corner and UV Max is the lower right corner. */
virtual EVROverlayError GetOverlayTextureBounds( VROverlayHandle_t ulOverlayHandle, VRTextureBounds_t *pOverlayTextureBounds ) = 0;
/** Gets render model to draw behind this overlay */
virtual uint32_t GetOverlayRenderModel( vr::VROverlayHandle_t ulOverlayHandle, char *pchValue, uint32_t unBufferSize, HmdColor_t *pColor, vr::EVROverlayError *pError ) = 0;
/** Sets render model to draw behind this overlay and the vertex color to use, pass null for pColor to match the overlays vertex color.
The model is scaled by the same amount as the overlay, with a default of 1m. */
virtual vr::EVROverlayError SetOverlayRenderModel( vr::VROverlayHandle_t ulOverlayHandle, const char *pchRenderModel, const HmdColor_t *pColor ) = 0;
/** Returns the transform type of this overlay. */
virtual EVROverlayError GetOverlayTransformType( VROverlayHandle_t ulOverlayHandle, VROverlayTransformType *peTransformType ) = 0;
@@ -2669,6 +2748,12 @@ namespace vr
/** Gets the transform information when the overlay is rendering on a component. */
virtual EVROverlayError GetOverlayTransformTrackedDeviceComponent( VROverlayHandle_t ulOverlayHandle, TrackedDeviceIndex_t *punDeviceIndex, char *pchComponentName, uint32_t unComponentNameSize ) = 0;
/** Gets the transform if it is relative to another overlay. Returns an error if the transform is some other type. */
virtual vr::EVROverlayError GetOverlayTransformOverlayRelative( VROverlayHandle_t ulOverlayHandle, VROverlayHandle_t *ulOverlayHandleParent, HmdMatrix34_t *pmatParentOverlayToOverlayTransform ) = 0;
/** Sets the transform to relative to the transform of the specified overlay. This overlays visibility will also track the parents visibility */
virtual vr::EVROverlayError SetOverlayTransformOverlayRelative( VROverlayHandle_t ulOverlayHandle, VROverlayHandle_t ulOverlayHandleParent, const HmdMatrix34_t *pmatParentOverlayToOverlayTransform ) = 0;
/** Shows the VR overlay. For dashboard overlays, only the Dashboard Manager is allowed to call this. */
virtual EVROverlayError ShowOverlay( VROverlayHandle_t ulOverlayHandle ) = 0;
@@ -2839,7 +2924,7 @@ namespace vr
virtual VRMessageOverlayResponse ShowMessageOverlay( const char* pchText, const char* pchCaption, const char* pchButton0Text, const char* pchButton1Text = nullptr, const char* pchButton2Text = nullptr, const char* pchButton3Text = nullptr ) = 0;
};
static const char * const IVROverlay_Version = "IVROverlay_014";
static const char * const IVROverlay_Version = "IVROverlay_016";
} // namespace vr
@@ -2853,16 +2938,7 @@ static const char * const k_pch_Controller_Component_Tip = "tip"; // F
static const char * const k_pch_Controller_Component_HandGrip = "handgrip"; // Neutral, ambidextrous hand-pose when holding controller. On plane between neutrally posed index finger and thumb
static const char * const k_pch_Controller_Component_Status = "status"; // 1:1 aspect ratio status area, with canonical [0,1] uv mapping
#if defined(__linux__) || defined(__APPLE__)
// The 32-bit version of gcc has the alignment requirement for uint64 and double set to
// 4 meaning that even with #pragma pack(8) these types will only be four-byte aligned.
// The 64-bit version of gcc has the alignment requirement for these types set to
// 8 meaning that unless we use #pragma pack(4) our structures will get bigger.
// The 64-bit structure packing has to match the 32-bit structure packing for each platform.
#pragma pack( push, 4 )
#else
#pragma pack( push, 8 )
#endif
/** Errors that can occur with the VR compositor */
enum EVRRenderModelError
@@ -2911,11 +2987,20 @@ struct RenderModel_Vertex_t
};
/** A texture map for use on a render model */
#if defined(__linux__) || defined(__APPLE__)
// This structure was originally defined mis-packed on Linux, preserved for
// compatibility.
#pragma pack( push, 4 )
#endif
struct RenderModel_TextureMap_t
{
uint16_t unWidth, unHeight; // width and height of the texture map in pixels
const uint8_t *rubTextureMapData; // Map texture data. All textures are RGBA with 8 bits per channel per pixel. Data size is width * height * 4ub
};
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( pop )
#endif
/** Session unique texture identifier. Rendermodels which share the same texture will have the same id.
IDs <0 denote the texture is not present */
@@ -2924,6 +3009,12 @@ typedef int32_t TextureID_t;
const TextureID_t INVALID_TEXTURE_ID = -1;
#if defined(__linux__) || defined(__APPLE__)
// This structure was originally defined mis-packed on Linux, preserved for
// compatibility.
#pragma pack( push, 4 )
#endif
struct RenderModel_t
{
const RenderModel_Vertex_t *rVertexData; // Vertex data for the mesh
@@ -2932,6 +3023,10 @@ struct RenderModel_t
uint32_t unTriangleCount; // Number of triangles in the mesh. Index count is 3 * TriangleCount
TextureID_t diffuseTextureId; // Session unique texture identifier. Rendermodels which share the same texture will have the same id. <0 == texture not present
};
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( pop )
#endif
struct RenderModel_ControllerMode_State_t
{
@@ -3168,7 +3263,7 @@ public:
* once SubmitScreenshot() is called.
* If Steam is not running, the paths will be in the user's
* documents folder under Documents\SteamVR\Screenshots.
* Other VR applications can call this to initate a
* Other VR applications can call this to initiate a
* screenshot outside of user control.
* The destination file names do not need an extension,
* will be replaced with the correct one for the format
@@ -3206,7 +3301,7 @@ public:
* submitted scene textures of the running application and
* write them into the preview image and a side-by-side file
* for the VR image.
* This is similiar to request screenshot, but doesn't ever
* This is similar to request screenshot, but doesn't ever
* talk to the application, just takes the shot and submits. */
virtual vr::EVRScreenshotError TakeStereoScreenshot( vr::ScreenshotHandle_t *pOutScreenshotHandle, const char *pchPreviewFilename, const char *pchVRFilename ) = 0;
@@ -3217,8 +3312,7 @@ public:
* function will display a notification to the user that the
* screenshot was taken. The paths should be full paths with
* extensions.
* File paths should be absolute including
* exntensions.
* File paths should be absolute including extensions.
* screenshotHandle can be k_unScreenshotHandleInvalid if this
* was a new shot taking by the app to be saved and not
* initiated by a user (achievement earned or something) */
@@ -3291,7 +3385,7 @@ namespace vr
/** Returns the name of the enum value for an EVRInitError. This function may be called outside of VR_Init()/VR_Shutdown(). */
VR_INTERFACE const char *VR_CALLTYPE VR_GetVRInitErrorAsSymbol( EVRInitError error );
/** Returns an english string for an EVRInitError. Applications should call VR_GetVRInitErrorAsSymbol instead and
/** Returns an English string for an EVRInitError. Applications should call VR_GetVRInitErrorAsSymbol instead and
* use that as a key to look up their own localized error message. This function may be called outside of VR_Init()/VR_Shutdown(). */
VR_INTERFACE const char *VR_CALLTYPE VR_GetVRInitErrorAsEnglishDescription( EVRInitError error );

View File

@@ -475,6 +475,11 @@ public struct IVRApplications
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _LaunchInternalProcess LaunchInternalProcess;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate uint _GetCurrentSceneProcessId();
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetCurrentSceneProcessId GetCurrentSceneProcessId;
}
[StructLayout(LayoutKind.Sequential)]
@@ -800,6 +805,11 @@ public struct IVRCompositor
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetMirrorTextureD3D11 GetMirrorTextureD3D11;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate void _ReleaseMirrorTextureD3D11(IntPtr pD3D11ShaderResourceView);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _ReleaseMirrorTextureD3D11 ReleaseMirrorTextureD3D11;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVRCompositorError _GetMirrorTextureGL(EVREye eEye, ref uint pglTextureId, IntPtr pglSharedTextureHandle);
[MarshalAs(UnmanagedType.FunctionPtr)]
@@ -841,7 +851,7 @@ public struct IVROverlay
internal _FindOverlay FindOverlay;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _CreateOverlay(string pchOverlayKey, string pchOverlayFriendlyName, ref ulong pOverlayHandle);
internal delegate EVROverlayError _CreateOverlay(string pchOverlayKey, string pchOverlayName, ref ulong pOverlayHandle);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _CreateOverlay CreateOverlay;
@@ -870,6 +880,11 @@ public struct IVROverlay
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetOverlayName GetOverlayName;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _SetOverlayName(ulong ulOverlayHandle, string pchName);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _SetOverlayName SetOverlayName;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _GetOverlayImageData(ulong ulOverlayHandle, IntPtr pvBuffer, uint unBufferSize, ref uint punWidth, ref uint punHeight);
[MarshalAs(UnmanagedType.FunctionPtr)]
@@ -980,6 +995,16 @@ public struct IVROverlay
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetOverlayTextureBounds GetOverlayTextureBounds;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate uint _GetOverlayRenderModel(ulong ulOverlayHandle, string pchValue, uint unBufferSize, ref HmdColor_t pColor, ref EVROverlayError pError);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetOverlayRenderModel GetOverlayRenderModel;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _SetOverlayRenderModel(ulong ulOverlayHandle, string pchRenderModel, ref HmdColor_t pColor);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _SetOverlayRenderModel SetOverlayRenderModel;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _GetOverlayTransformType(ulong ulOverlayHandle, ref VROverlayTransformType peTransformType);
[MarshalAs(UnmanagedType.FunctionPtr)]
@@ -1015,6 +1040,16 @@ public struct IVROverlay
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetOverlayTransformTrackedDeviceComponent GetOverlayTransformTrackedDeviceComponent;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _GetOverlayTransformOverlayRelative(ulong ulOverlayHandle, ref ulong ulOverlayHandleParent, ref HmdMatrix34_t pmatParentOverlayToOverlayTransform);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _GetOverlayTransformOverlayRelative GetOverlayTransformOverlayRelative;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _SetOverlayTransformOverlayRelative(ulong ulOverlayHandle, ulong ulOverlayHandleParent, ref HmdMatrix34_t pmatParentOverlayToOverlayTransform);
[MarshalAs(UnmanagedType.FunctionPtr)]
internal _SetOverlayTransformOverlayRelative SetOverlayTransformOverlayRelative;
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate EVROverlayError _ShowOverlay(ulong ulOverlayHandle);
[MarshalAs(UnmanagedType.FunctionPtr)]
@@ -1593,8 +1628,32 @@ public class CVRSystem
IntPtr result = FnTable.GetPropErrorNameFromEnum(error);
return Marshal.PtrToStringAnsi(result);
}
// This is a terrible hack to workaround the fact that VRControllerState_t and VREvent_t were
// originally mis-compiled with the wrong packing for Linux and OSX.
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate bool _PollNextEventPacked(ref VREvent_t_Packed pEvent,uint uncbVREvent);
[StructLayout(LayoutKind.Explicit)]
struct PollNextEventUnion
{
[FieldOffset(0)]
public IVRSystem._PollNextEvent pPollNextEvent;
[FieldOffset(0)]
public _PollNextEventPacked pPollNextEventPacked;
}
public bool PollNextEvent(ref VREvent_t pEvent,uint uncbVREvent)
{
if ((System.Environment.OSVersion.Platform == System.PlatformID.MacOSX) ||
(System.Environment.OSVersion.Platform == System.PlatformID.Unix))
{
PollNextEventUnion u;
VREvent_t_Packed event_packed = new VREvent_t_Packed();
u.pPollNextEventPacked = null;
u.pPollNextEvent = FnTable.PollNextEvent;
bool packed_result = u.pPollNextEventPacked(ref event_packed,(uint)System.Runtime.InteropServices.Marshal.SizeOf(typeof(VREvent_t_Packed)));
event_packed.Unpack(ref pEvent);
return packed_result;
}
bool result = FnTable.PollNextEvent(ref pEvent,uncbVREvent);
return result;
}
@@ -1613,13 +1672,61 @@ public class CVRSystem
HiddenAreaMesh_t result = FnTable.GetHiddenAreaMesh(eEye,type);
return result;
}
// This is a terrible hack to workaround the fact that VRControllerState_t and VREvent_t were
// originally mis-compiled with the wrong packing for Linux and OSX.
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate bool _GetControllerStatePacked(uint unControllerDeviceIndex,ref VRControllerState_t_Packed pControllerState,uint unControllerStateSize);
[StructLayout(LayoutKind.Explicit)]
struct GetControllerStateUnion
{
[FieldOffset(0)]
public IVRSystem._GetControllerState pGetControllerState;
[FieldOffset(0)]
public _GetControllerStatePacked pGetControllerStatePacked;
}
public bool GetControllerState(uint unControllerDeviceIndex,ref VRControllerState_t pControllerState,uint unControllerStateSize)
{
if ((System.Environment.OSVersion.Platform == System.PlatformID.MacOSX) ||
(System.Environment.OSVersion.Platform == System.PlatformID.Unix))
{
GetControllerStateUnion u;
VRControllerState_t_Packed state_packed = new VRControllerState_t_Packed(pControllerState);
u.pGetControllerStatePacked = null;
u.pGetControllerState = FnTable.GetControllerState;
bool packed_result = u.pGetControllerStatePacked(unControllerDeviceIndex,ref state_packed,(uint)System.Runtime.InteropServices.Marshal.SizeOf(typeof(VRControllerState_t_Packed)));
state_packed.Unpack(ref pControllerState);
return packed_result;
}
bool result = FnTable.GetControllerState(unControllerDeviceIndex,ref pControllerState,unControllerStateSize);
return result;
}
// This is a terrible hack to workaround the fact that VRControllerState_t and VREvent_t were
// originally mis-compiled with the wrong packing for Linux and OSX.
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate bool _GetControllerStateWithPosePacked(ETrackingUniverseOrigin eOrigin,uint unControllerDeviceIndex,ref VRControllerState_t_Packed pControllerState,uint unControllerStateSize,ref TrackedDevicePose_t pTrackedDevicePose);
[StructLayout(LayoutKind.Explicit)]
struct GetControllerStateWithPoseUnion
{
[FieldOffset(0)]
public IVRSystem._GetControllerStateWithPose pGetControllerStateWithPose;
[FieldOffset(0)]
public _GetControllerStateWithPosePacked pGetControllerStateWithPosePacked;
}
public bool GetControllerStateWithPose(ETrackingUniverseOrigin eOrigin,uint unControllerDeviceIndex,ref VRControllerState_t pControllerState,uint unControllerStateSize,ref TrackedDevicePose_t pTrackedDevicePose)
{
if ((System.Environment.OSVersion.Platform == System.PlatformID.MacOSX) ||
(System.Environment.OSVersion.Platform == System.PlatformID.Unix))
{
GetControllerStateWithPoseUnion u;
VRControllerState_t_Packed state_packed = new VRControllerState_t_Packed(pControllerState);
u.pGetControllerStateWithPosePacked = null;
u.pGetControllerStateWithPose = FnTable.GetControllerStateWithPose;
bool packed_result = u.pGetControllerStateWithPosePacked(eOrigin,unControllerDeviceIndex,ref state_packed,(uint)System.Runtime.InteropServices.Marshal.SizeOf(typeof(VRControllerState_t_Packed)),ref pTrackedDevicePose);
state_packed.Unpack(ref pControllerState);
return packed_result;
}
bool result = FnTable.GetControllerStateWithPose(eOrigin,unControllerDeviceIndex,ref pControllerState,unControllerStateSize,ref pTrackedDevicePose);
return result;
}
@@ -1939,6 +2046,11 @@ public class CVRApplications
EVRApplicationError result = FnTable.LaunchInternalProcess(pchBinaryPath,pchArguments,pchWorkingDirectory);
return result;
}
public uint GetCurrentSceneProcessId()
{
uint result = FnTable.GetCurrentSceneProcessId();
return result;
}
}
@@ -2269,6 +2381,10 @@ public class CVRCompositor
EVRCompositorError result = FnTable.GetMirrorTextureD3D11(eEye,pD3D11DeviceOrResource,ref ppD3D11ShaderResourceView);
return result;
}
public void ReleaseMirrorTextureD3D11(IntPtr pD3D11ShaderResourceView)
{
FnTable.ReleaseMirrorTextureD3D11(pD3D11ShaderResourceView);
}
public EVRCompositorError GetMirrorTextureGL(EVREye eEye,ref uint pglTextureId,IntPtr pglSharedTextureHandle)
{
pglTextureId = 0;
@@ -2314,10 +2430,10 @@ public class CVROverlay
EVROverlayError result = FnTable.FindOverlay(pchOverlayKey,ref pOverlayHandle);
return result;
}
public EVROverlayError CreateOverlay(string pchOverlayKey,string pchOverlayFriendlyName,ref ulong pOverlayHandle)
public EVROverlayError CreateOverlay(string pchOverlayKey,string pchOverlayName,ref ulong pOverlayHandle)
{
pOverlayHandle = 0;
EVROverlayError result = FnTable.CreateOverlay(pchOverlayKey,pchOverlayFriendlyName,ref pOverlayHandle);
EVROverlayError result = FnTable.CreateOverlay(pchOverlayKey,pchOverlayName,ref pOverlayHandle);
return result;
}
public EVROverlayError DestroyOverlay(ulong ulOverlayHandle)
@@ -2345,6 +2461,11 @@ public class CVROverlay
uint result = FnTable.GetOverlayName(ulOverlayHandle,pchValue,unBufferSize,ref pError);
return result;
}
public EVROverlayError SetOverlayName(ulong ulOverlayHandle,string pchName)
{
EVROverlayError result = FnTable.SetOverlayName(ulOverlayHandle,pchName);
return result;
}
public EVROverlayError GetOverlayImageData(ulong ulOverlayHandle,IntPtr pvBuffer,uint unBufferSize,ref uint punWidth,ref uint punHeight)
{
punWidth = 0;
@@ -2467,6 +2588,16 @@ public class CVROverlay
EVROverlayError result = FnTable.GetOverlayTextureBounds(ulOverlayHandle,ref pOverlayTextureBounds);
return result;
}
public uint GetOverlayRenderModel(ulong ulOverlayHandle,string pchValue,uint unBufferSize,ref HmdColor_t pColor,ref EVROverlayError pError)
{
uint result = FnTable.GetOverlayRenderModel(ulOverlayHandle,pchValue,unBufferSize,ref pColor,ref pError);
return result;
}
public EVROverlayError SetOverlayRenderModel(ulong ulOverlayHandle,string pchRenderModel,ref HmdColor_t pColor)
{
EVROverlayError result = FnTable.SetOverlayRenderModel(ulOverlayHandle,pchRenderModel,ref pColor);
return result;
}
public EVROverlayError GetOverlayTransformType(ulong ulOverlayHandle,ref VROverlayTransformType peTransformType)
{
EVROverlayError result = FnTable.GetOverlayTransformType(ulOverlayHandle,ref peTransformType);
@@ -2504,6 +2635,17 @@ public class CVROverlay
EVROverlayError result = FnTable.GetOverlayTransformTrackedDeviceComponent(ulOverlayHandle,ref punDeviceIndex,pchComponentName,unComponentNameSize);
return result;
}
public EVROverlayError GetOverlayTransformOverlayRelative(ulong ulOverlayHandle,ref ulong ulOverlayHandleParent,ref HmdMatrix34_t pmatParentOverlayToOverlayTransform)
{
ulOverlayHandleParent = 0;
EVROverlayError result = FnTable.GetOverlayTransformOverlayRelative(ulOverlayHandle,ref ulOverlayHandleParent,ref pmatParentOverlayToOverlayTransform);
return result;
}
public EVROverlayError SetOverlayTransformOverlayRelative(ulong ulOverlayHandle,ulong ulOverlayHandleParent,ref HmdMatrix34_t pmatParentOverlayToOverlayTransform)
{
EVROverlayError result = FnTable.SetOverlayTransformOverlayRelative(ulOverlayHandle,ulOverlayHandleParent,ref pmatParentOverlayToOverlayTransform);
return result;
}
public EVROverlayError ShowOverlay(ulong ulOverlayHandle)
{
EVROverlayError result = FnTable.ShowOverlay(ulOverlayHandle);
@@ -2524,8 +2666,32 @@ public class CVROverlay
EVROverlayError result = FnTable.GetTransformForOverlayCoordinates(ulOverlayHandle,eTrackingOrigin,coordinatesInOverlay,ref pmatTransform);
return result;
}
// This is a terrible hack to workaround the fact that VRControllerState_t and VREvent_t were
// originally mis-compiled with the wrong packing for Linux and OSX.
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate bool _PollNextOverlayEventPacked(ulong ulOverlayHandle,ref VREvent_t_Packed pEvent,uint uncbVREvent);
[StructLayout(LayoutKind.Explicit)]
struct PollNextOverlayEventUnion
{
[FieldOffset(0)]
public IVROverlay._PollNextOverlayEvent pPollNextOverlayEvent;
[FieldOffset(0)]
public _PollNextOverlayEventPacked pPollNextOverlayEventPacked;
}
public bool PollNextOverlayEvent(ulong ulOverlayHandle,ref VREvent_t pEvent,uint uncbVREvent)
{
if ((System.Environment.OSVersion.Platform == System.PlatformID.MacOSX) ||
(System.Environment.OSVersion.Platform == System.PlatformID.Unix))
{
PollNextOverlayEventUnion u;
VREvent_t_Packed event_packed = new VREvent_t_Packed();
u.pPollNextOverlayEventPacked = null;
u.pPollNextOverlayEvent = FnTable.PollNextOverlayEvent;
bool packed_result = u.pPollNextOverlayEventPacked(ulOverlayHandle,ref event_packed,(uint)System.Runtime.InteropServices.Marshal.SizeOf(typeof(VREvent_t_Packed)));
event_packed.Unpack(ref pEvent);
return packed_result;
}
bool result = FnTable.PollNextOverlayEvent(ulOverlayHandle,ref pEvent,uncbVREvent);
return result;
}
@@ -2776,8 +2942,32 @@ public class CVRRenderModels
uint result = FnTable.GetComponentRenderModelName(pchRenderModelName,pchComponentName,pchComponentRenderModelName,unComponentRenderModelNameLen);
return result;
}
// This is a terrible hack to workaround the fact that VRControllerState_t and VREvent_t were
// originally mis-compiled with the wrong packing for Linux and OSX.
[UnmanagedFunctionPointer(CallingConvention.StdCall)]
internal delegate bool _GetComponentStatePacked(string pchRenderModelName,string pchComponentName,ref VRControllerState_t_Packed pControllerState,ref RenderModel_ControllerMode_State_t pState,ref RenderModel_ComponentState_t pComponentState);
[StructLayout(LayoutKind.Explicit)]
struct GetComponentStateUnion
{
[FieldOffset(0)]
public IVRRenderModels._GetComponentState pGetComponentState;
[FieldOffset(0)]
public _GetComponentStatePacked pGetComponentStatePacked;
}
public bool GetComponentState(string pchRenderModelName,string pchComponentName,ref VRControllerState_t pControllerState,ref RenderModel_ControllerMode_State_t pState,ref RenderModel_ComponentState_t pComponentState)
{
if ((System.Environment.OSVersion.Platform == System.PlatformID.MacOSX) ||
(System.Environment.OSVersion.Platform == System.PlatformID.Unix))
{
GetComponentStateUnion u;
VRControllerState_t_Packed state_packed = new VRControllerState_t_Packed(pControllerState);
u.pGetComponentStatePacked = null;
u.pGetComponentState = FnTable.GetComponentState;
bool packed_result = u.pGetComponentStatePacked(pchRenderModelName,pchComponentName,ref state_packed,ref pState,ref pComponentState);
state_packed.Unpack(ref pControllerState);
return packed_result;
}
bool result = FnTable.GetComponentState(pchRenderModelName,pchComponentName,ref pControllerState,ref pState,ref pComponentState);
return result;
}
@@ -2986,6 +3176,8 @@ public enum ETextureType
DirectX = 0,
OpenGL = 1,
Vulkan = 2,
IOSurface = 3,
DirectX12 = 4,
}
public enum EColorSpace
{
@@ -3008,6 +3200,7 @@ public enum ETrackedDeviceClass
Controller = 2,
GenericTracker = 3,
TrackingReference = 4,
DisplayRedirect = 5,
}
public enum ETrackedControllerRole
{
@@ -3058,6 +3251,8 @@ public enum ETrackedDeviceProperty
Prop_DriverVersion_String = 1031,
Prop_Firmware_ForceUpdateRequired_Bool = 1032,
Prop_ViveSystemButtonFixRequired_Bool = 1033,
Prop_ParentDriver_Uint64 = 1034,
Prop_ResourceRoot_String = 1035,
Prop_ReportsTimeSinceVSync_Bool = 2000,
Prop_SecondsFromVsyncToPhotons_Float = 2001,
Prop_DisplayFrequency_Float = 2002,
@@ -3096,6 +3291,11 @@ public enum ETrackedDeviceProperty
Prop_ScreenshotVerticalFieldOfViewDegrees_Float = 2035,
Prop_DisplaySuppressed_Bool = 2036,
Prop_DisplayAllowNightMode_Bool = 2037,
Prop_DisplayMCImageWidth_Int32 = 2038,
Prop_DisplayMCImageHeight_Int32 = 2039,
Prop_DisplayMCImageNumChannels_Int32 = 2040,
Prop_DisplayMCImageData_Binary = 2041,
Prop_SecondsFromPhotonsToVblank_Float = 2042,
Prop_AttachedDeviceId_String = 3000,
Prop_SupportedButtons_Uint64 = 3001,
Prop_Axis0Type_Int32 = 3002,
@@ -3120,6 +3320,15 @@ public enum ETrackedDeviceProperty
Prop_NamedIconPathDeviceNotReady_String = 5006,
Prop_NamedIconPathDeviceStandby_String = 5007,
Prop_NamedIconPathDeviceAlertLow_String = 5008,
Prop_DisplayHiddenArea_Binary_Start = 5100,
Prop_DisplayHiddenArea_Binary_End = 5150,
Prop_UserConfigPath_String = 6000,
Prop_InstallPath_String = 6001,
Prop_HasDisplayComponent_Bool = 6002,
Prop_HasControllerComponent_Bool = 6003,
Prop_HasCameraComponent_Bool = 6004,
Prop_HasDriverDirectModeComponent_Bool = 6005,
Prop_HasVirtualDisplayComponent_Bool = 6006,
Prop_VendorSpecific_Reserved_Start = 10000,
Prop_VendorSpecific_Reserved_End = 10999,
}
@@ -3136,6 +3345,7 @@ public enum ETrackedPropertyError
TrackedProp_StringExceedsMaximumLength = 8,
TrackedProp_NotYetAvailable = 9,
TrackedProp_PermissionDenied = 10,
TrackedProp_InvalidOperation = 11,
}
public enum EVRSubmitFlags
{
@@ -3170,6 +3380,7 @@ public enum EVREventType
VREvent_TrackedDeviceRoleChanged = 108,
VREvent_WatchdogWakeUpRequested = 109,
VREvent_LensDistortionChanged = 110,
VREvent_PropertyChanged = 111,
VREvent_ButtonPress = 200,
VREvent_ButtonUnpress = 201,
VREvent_ButtonTouch = 202,
@@ -3216,6 +3427,7 @@ public enum EVREventType
VREvent_ScreenshotFailed = 522,
VREvent_SubmitScreenshotToDashboard = 523,
VREvent_ScreenshotProgressToDashboard = 524,
VREvent_PrimaryDashboardDeviceChanged = 525,
VREvent_Notification_Shown = 600,
VREvent_Notification_Hidden = 601,
VREvent_Notification_BeginInteraction = 602,
@@ -3249,6 +3461,9 @@ public enum EVREventType
VREvent_ApplicationTransitionNewAppStarted = 1302,
VREvent_ApplicationListUpdated = 1303,
VREvent_ApplicationMimeTypeLoad = 1304,
VREvent_ApplicationTransitionNewAppLaunchComplete = 1305,
VREvent_ProcessConnected = 1306,
VREvent_ProcessDisconnected = 1307,
VREvent_Compositor_MirrorWindowShown = 1400,
VREvent_Compositor_MirrorWindowHidden = 1401,
VREvent_Compositor_ChaperoneBoundsShown = 1410,
@@ -3305,6 +3520,7 @@ public enum EHiddenAreaMeshType
k_eHiddenAreaMesh_Standard = 0,
k_eHiddenAreaMesh_Inverse = 1,
k_eHiddenAreaMesh_LineLoop = 2,
k_eHiddenAreaMesh_Max = 3,
}
public enum EVRControllerAxisType
{
@@ -3360,7 +3576,8 @@ public enum EVRApplicationType
VRApplication_Utility = 4,
VRApplication_VRMonitor = 5,
VRApplication_SteamWatchdog = 6,
VRApplication_Max = 7,
VRApplication_Bootstrapper = 7,
VRApplication_Max = 8,
}
public enum EVRFirmwareError
{
@@ -3536,6 +3753,7 @@ public enum EVRApplicationProperty
IsDashboardOverlay_Bool = 60,
IsTemplate_Bool = 61,
IsInstanced_Bool = 62,
IsInternal_Bool = 63,
LastLaunchTime_Uint64 = 70,
}
public enum EVRApplicationTransitionState
@@ -3863,6 +4081,12 @@ public enum EVRScreenshotError
public uint m_nFormat;
public uint m_nSampleCount;
}
[StructLayout(LayoutKind.Sequential)] public struct D3D12TextureData_t
{
public IntPtr m_pResource; // struct ID3D12Resource *
public IntPtr m_pCommandQueue; // struct ID3D12CommandQueue *
public uint m_nNodeMask;
}
[StructLayout(LayoutKind.Sequential)] public struct VREvent_Controller_t
{
public uint button;
@@ -3960,6 +4184,11 @@ public enum EVRScreenshotError
{
public uint unVRMessageOverlayResponse;
}
[StructLayout(LayoutKind.Sequential)] public struct VREvent_Property_t
{
public ulong container;
public ETrackedDeviceProperty prop;
}
[StructLayout(LayoutKind.Sequential)] public struct VREvent_t
{
public uint eventType;
@@ -3967,6 +4196,28 @@ public enum EVRScreenshotError
public float eventAgeSeconds;
public VREvent_Data_t data;
}
// This structure is for backwards binary compatibility on Linux and OSX only
[StructLayout(LayoutKind.Sequential, Pack = 4)] public struct VREvent_t_Packed
{
public uint eventType;
public uint trackedDeviceIndex;
public float eventAgeSeconds;
public VREvent_Data_t data;
public VREvent_t_Packed(VREvent_t unpacked)
{
this.eventType = unpacked.eventType;
this.trackedDeviceIndex = unpacked.trackedDeviceIndex;
this.eventAgeSeconds = unpacked.eventAgeSeconds;
this.data = unpacked.data;
}
public void Unpack(ref VREvent_t unpacked)
{
unpacked.eventType = this.eventType;
unpacked.trackedDeviceIndex = this.trackedDeviceIndex;
unpacked.eventAgeSeconds = this.eventAgeSeconds;
unpacked.data = this.data;
}
}
[StructLayout(LayoutKind.Sequential)] public struct HiddenAreaMesh_t
{
public IntPtr pVertexData; // const struct vr::HmdVector2_t *
@@ -3988,6 +4239,40 @@ public enum EVRScreenshotError
public VRControllerAxis_t rAxis3;
public VRControllerAxis_t rAxis4;
}
// This structure is for backwards binary compatibility on Linux and OSX only
[StructLayout(LayoutKind.Sequential, Pack = 4)] public struct VRControllerState_t_Packed
{
public uint unPacketNum;
public ulong ulButtonPressed;
public ulong ulButtonTouched;
public VRControllerAxis_t rAxis0; //VRControllerAxis_t[5]
public VRControllerAxis_t rAxis1;
public VRControllerAxis_t rAxis2;
public VRControllerAxis_t rAxis3;
public VRControllerAxis_t rAxis4;
public VRControllerState_t_Packed(VRControllerState_t unpacked)
{
this.unPacketNum = unpacked.unPacketNum;
this.ulButtonPressed = unpacked.ulButtonPressed;
this.ulButtonTouched = unpacked.ulButtonTouched;
this.rAxis0 = unpacked.rAxis0;
this.rAxis1 = unpacked.rAxis1;
this.rAxis2 = unpacked.rAxis2;
this.rAxis3 = unpacked.rAxis3;
this.rAxis4 = unpacked.rAxis4;
}
public void Unpack(ref VRControllerState_t unpacked)
{
unpacked.unPacketNum = this.unPacketNum;
unpacked.ulButtonPressed = this.ulButtonPressed;
unpacked.ulButtonTouched = this.ulButtonTouched;
unpacked.rAxis0 = this.rAxis0;
unpacked.rAxis1 = this.rAxis1;
unpacked.rAxis2 = this.rAxis2;
unpacked.rAxis3 = this.rAxis3;
unpacked.rAxis4 = this.rAxis4;
}
}
[StructLayout(LayoutKind.Sequential)] public struct Compositor_OverlaySettings
{
public uint size;
@@ -4116,6 +4401,25 @@ public enum EVRScreenshotError
public char unHeight;
public IntPtr rubTextureMapData; // const uint8_t *
}
// This structure is for backwards binary compatibility on Linux and OSX only
[StructLayout(LayoutKind.Sequential, Pack = 4)] public struct RenderModel_TextureMap_t_Packed
{
public char unWidth;
public char unHeight;
public IntPtr rubTextureMapData; // const uint8_t *
public RenderModel_TextureMap_t_Packed(RenderModel_TextureMap_t unpacked)
{
this.unWidth = unpacked.unWidth;
this.unHeight = unpacked.unHeight;
this.rubTextureMapData = unpacked.rubTextureMapData;
}
public void Unpack(ref RenderModel_TextureMap_t unpacked)
{
unpacked.unWidth = this.unWidth;
unpacked.unHeight = this.unHeight;
unpacked.rubTextureMapData = this.rubTextureMapData;
}
}
[StructLayout(LayoutKind.Sequential)] public struct RenderModel_t
{
public IntPtr rVertexData; // const struct vr::RenderModel_Vertex_t *
@@ -4124,6 +4428,31 @@ public enum EVRScreenshotError
public uint unTriangleCount;
public int diffuseTextureId;
}
// This structure is for backwards binary compatibility on Linux and OSX only
[StructLayout(LayoutKind.Sequential, Pack = 4)] public struct RenderModel_t_Packed
{
public IntPtr rVertexData; // const struct vr::RenderModel_Vertex_t *
public uint unVertexCount;
public IntPtr rIndexData; // const uint16_t *
public uint unTriangleCount;
public int diffuseTextureId;
public RenderModel_t_Packed(RenderModel_t unpacked)
{
this.rVertexData = unpacked.rVertexData;
this.unVertexCount = unpacked.unVertexCount;
this.rIndexData = unpacked.rIndexData;
this.unTriangleCount = unpacked.unTriangleCount;
this.diffuseTextureId = unpacked.diffuseTextureId;
}
public void Unpack(ref RenderModel_t unpacked)
{
unpacked.rVertexData = this.rVertexData;
unpacked.unVertexCount = this.unVertexCount;
unpacked.rIndexData = this.rIndexData;
unpacked.unTriangleCount = this.unTriangleCount;
unpacked.diffuseTextureId = this.diffuseTextureId;
}
}
[StructLayout(LayoutKind.Sequential)] public struct RenderModel_ControllerMode_State_t
{
[MarshalAs(UnmanagedType.I1)]
@@ -4200,6 +4529,20 @@ public class OpenVR
public const uint k_unMaxTrackedDeviceCount = 16;
public const uint k_unTrackedDeviceIndexOther = 4294967294;
public const uint k_unTrackedDeviceIndexInvalid = 4294967295;
public const ulong k_ulInvalidPropertyContainer = 0;
public const uint k_unInvalidPropertyTag = 0;
public const uint k_unFloatPropertyTag = 1;
public const uint k_unInt32PropertyTag = 2;
public const uint k_unUint64PropertyTag = 3;
public const uint k_unBoolPropertyTag = 4;
public const uint k_unStringPropertyTag = 5;
public const uint k_unHmdMatrix34PropertyTag = 20;
public const uint k_unHmdMatrix44PropertyTag = 21;
public const uint k_unHmdVector3PropertyTag = 22;
public const uint k_unHmdVector4PropertyTag = 23;
public const uint k_unHiddenAreaPropertyTag = 30;
public const uint k_unOpenVRInternalReserved_Start = 1000;
public const uint k_unOpenVRInternalReserved_End = 10000;
public const uint k_unMaxPropertyStringSize = 32768;
public const uint k_unControllerStateAxisCount = 5;
public const ulong k_ulOverlayHandleInvalid = 0;
@@ -4213,12 +4556,12 @@ public class OpenVR
public const string IVRApplications_Version = "IVRApplications_006";
public const string IVRChaperone_Version = "IVRChaperone_003";
public const string IVRChaperoneSetup_Version = "IVRChaperoneSetup_005";
public const string IVRCompositor_Version = "IVRCompositor_019";
public const string IVRCompositor_Version = "IVRCompositor_020";
public const uint k_unVROverlayMaxKeyLength = 128;
public const uint k_unVROverlayMaxNameLength = 128;
public const uint k_unMaxOverlayCount = 64;
public const uint k_unMaxOverlayIntersectionMaskPrimitivesCount = 32;
public const string IVROverlay_Version = "IVROverlay_014";
public const string IVROverlay_Version = "IVROverlay_016";
public const string k_pch_Controller_Component_GDC2015 = "gdc2015";
public const string k_pch_Controller_Component_Base = "base";
public const string k_pch_Controller_Component_Tip = "tip";
@@ -4235,7 +4578,6 @@ public class OpenVR
public const string k_pch_SteamVR_ForcedHmdKey_String = "forcedHmd";
public const string k_pch_SteamVR_DisplayDebug_Bool = "displayDebug";
public const string k_pch_SteamVR_DebugProcessPipe_String = "debugProcessPipe";
public const string k_pch_SteamVR_EnableDistortion_Bool = "enableDistortion";
public const string k_pch_SteamVR_DisplayDebugX_Int32 = "displayDebugX";
public const string k_pch_SteamVR_DisplayDebugY_Int32 = "displayDebugY";
public const string k_pch_SteamVR_SendSystemButtonToAllApps_Bool = "sendSystemButtonToAllApps";
@@ -4280,7 +4622,6 @@ public class OpenVR
public const string k_pch_Lighthouse_PrimaryBasestation_Int32 = "primarybasestation";
public const string k_pch_Lighthouse_DBHistory_Bool = "dbhistory";
public const string k_pch_Null_Section = "driver_null";
public const string k_pch_Null_EnableNullDriver_Bool = "enable";
public const string k_pch_Null_SerialNumber_String = "serialNumber";
public const string k_pch_Null_ModelNumber_String = "modelNumber";
public const string k_pch_Null_WindowX_Int32 = "windowX";
@@ -4350,6 +4691,7 @@ public class OpenVR
public const string k_pch_Dashboard_EnableDashboard_Bool = "enableDashboard";
public const string k_pch_Dashboard_ArcadeMode_Bool = "arcadeMode";
public const string k_pch_modelskin_Section = "modelskins";
public const string k_pch_Driver_Enable_Bool = "enable";
public const string IVRScreenshots_Version = "IVRScreenshots_001";
public const string IVRResources_Version = "IVRResources_001";

View File

@@ -3,6 +3,8 @@
,{"typedef": "vr::glUInt_t","type": "uint32_t"}
,{"typedef": "vr::SharedTextureHandle_t","type": "uint64_t"}
,{"typedef": "vr::TrackedDeviceIndex_t","type": "uint32_t"}
,{"typedef": "vr::PropertyContainerHandle_t","type": "uint64_t"}
,{"typedef": "vr::PropertyTypeTag_t","type": "uint32_t"}
,{"typedef": "vr::VREvent_Data_t","type": "union VREvent_Data_t"}
,{"typedef": "vr::VRControllerState_t","type": "struct vr::VRControllerState001_t"}
,{"typedef": "vr::VROverlayHandle_t","type": "uint64_t"}
@@ -37,6 +39,8 @@
{"name": "TextureType_DirectX","value": "0"}
,{"name": "TextureType_OpenGL","value": "1"}
,{"name": "TextureType_Vulkan","value": "2"}
,{"name": "TextureType_IOSurface","value": "3"}
,{"name": "TextureType_DirectX12","value": "4"}
]}
, {"enumname": "vr::EColorSpace","values": [
{"name": "ColorSpace_Auto","value": "0"}
@@ -56,6 +60,7 @@
,{"name": "TrackedDeviceClass_Controller","value": "2"}
,{"name": "TrackedDeviceClass_GenericTracker","value": "3"}
,{"name": "TrackedDeviceClass_TrackingReference","value": "4"}
,{"name": "TrackedDeviceClass_DisplayRedirect","value": "5"}
]}
, {"enumname": "vr::ETrackedControllerRole","values": [
{"name": "TrackedControllerRole_Invalid","value": "0"}
@@ -103,6 +108,8 @@
,{"name": "Prop_DriverVersion_String","value": "1031"}
,{"name": "Prop_Firmware_ForceUpdateRequired_Bool","value": "1032"}
,{"name": "Prop_ViveSystemButtonFixRequired_Bool","value": "1033"}
,{"name": "Prop_ParentDriver_Uint64","value": "1034"}
,{"name": "Prop_ResourceRoot_String","value": "1035"}
,{"name": "Prop_ReportsTimeSinceVSync_Bool","value": "2000"}
,{"name": "Prop_SecondsFromVsyncToPhotons_Float","value": "2001"}
,{"name": "Prop_DisplayFrequency_Float","value": "2002"}
@@ -141,6 +148,11 @@
,{"name": "Prop_ScreenshotVerticalFieldOfViewDegrees_Float","value": "2035"}
,{"name": "Prop_DisplaySuppressed_Bool","value": "2036"}
,{"name": "Prop_DisplayAllowNightMode_Bool","value": "2037"}
,{"name": "Prop_DisplayMCImageWidth_Int32","value": "2038"}
,{"name": "Prop_DisplayMCImageHeight_Int32","value": "2039"}
,{"name": "Prop_DisplayMCImageNumChannels_Int32","value": "2040"}
,{"name": "Prop_DisplayMCImageData_Binary","value": "2041"}
,{"name": "Prop_SecondsFromPhotonsToVblank_Float","value": "2042"}
,{"name": "Prop_AttachedDeviceId_String","value": "3000"}
,{"name": "Prop_SupportedButtons_Uint64","value": "3001"}
,{"name": "Prop_Axis0Type_Int32","value": "3002"}
@@ -165,6 +177,15 @@
,{"name": "Prop_NamedIconPathDeviceNotReady_String","value": "5006"}
,{"name": "Prop_NamedIconPathDeviceStandby_String","value": "5007"}
,{"name": "Prop_NamedIconPathDeviceAlertLow_String","value": "5008"}
,{"name": "Prop_DisplayHiddenArea_Binary_Start","value": "5100"}
,{"name": "Prop_DisplayHiddenArea_Binary_End","value": "5150"}
,{"name": "Prop_UserConfigPath_String","value": "6000"}
,{"name": "Prop_InstallPath_String","value": "6001"}
,{"name": "Prop_HasDisplayComponent_Bool","value": "6002"}
,{"name": "Prop_HasControllerComponent_Bool","value": "6003"}
,{"name": "Prop_HasCameraComponent_Bool","value": "6004"}
,{"name": "Prop_HasDriverDirectModeComponent_Bool","value": "6005"}
,{"name": "Prop_HasVirtualDisplayComponent_Bool","value": "6006"}
,{"name": "Prop_VendorSpecific_Reserved_Start","value": "10000"}
,{"name": "Prop_VendorSpecific_Reserved_End","value": "10999"}
]}
@@ -180,6 +201,7 @@
,{"name": "TrackedProp_StringExceedsMaximumLength","value": "8"}
,{"name": "TrackedProp_NotYetAvailable","value": "9"}
,{"name": "TrackedProp_PermissionDenied","value": "10"}
,{"name": "TrackedProp_InvalidOperation","value": "11"}
]}
, {"enumname": "vr::EVRSubmitFlags","values": [
{"name": "Submit_Default","value": "0"}
@@ -211,6 +233,7 @@
,{"name": "VREvent_TrackedDeviceRoleChanged","value": "108"}
,{"name": "VREvent_WatchdogWakeUpRequested","value": "109"}
,{"name": "VREvent_LensDistortionChanged","value": "110"}
,{"name": "VREvent_PropertyChanged","value": "111"}
,{"name": "VREvent_ButtonPress","value": "200"}
,{"name": "VREvent_ButtonUnpress","value": "201"}
,{"name": "VREvent_ButtonTouch","value": "202"}
@@ -257,6 +280,7 @@
,{"name": "VREvent_ScreenshotFailed","value": "522"}
,{"name": "VREvent_SubmitScreenshotToDashboard","value": "523"}
,{"name": "VREvent_ScreenshotProgressToDashboard","value": "524"}
,{"name": "VREvent_PrimaryDashboardDeviceChanged","value": "525"}
,{"name": "VREvent_Notification_Shown","value": "600"}
,{"name": "VREvent_Notification_Hidden","value": "601"}
,{"name": "VREvent_Notification_BeginInteraction","value": "602"}
@@ -290,6 +314,9 @@
,{"name": "VREvent_ApplicationTransitionNewAppStarted","value": "1302"}
,{"name": "VREvent_ApplicationListUpdated","value": "1303"}
,{"name": "VREvent_ApplicationMimeTypeLoad","value": "1304"}
,{"name": "VREvent_ApplicationTransitionNewAppLaunchComplete","value": "1305"}
,{"name": "VREvent_ProcessConnected","value": "1306"}
,{"name": "VREvent_ProcessDisconnected","value": "1307"}
,{"name": "VREvent_Compositor_MirrorWindowShown","value": "1400"}
,{"name": "VREvent_Compositor_MirrorWindowHidden","value": "1401"}
,{"name": "VREvent_Compositor_ChaperoneBoundsShown","value": "1410"}
@@ -342,6 +369,7 @@
{"name": "k_eHiddenAreaMesh_Standard","value": "0"}
,{"name": "k_eHiddenAreaMesh_Inverse","value": "1"}
,{"name": "k_eHiddenAreaMesh_LineLoop","value": "2"}
,{"name": "k_eHiddenAreaMesh_Max","value": "3"}
]}
, {"enumname": "vr::EVRControllerAxisType","values": [
{"name": "k_eControllerAxis_None","value": "0"}
@@ -392,7 +420,8 @@
,{"name": "VRApplication_Utility","value": "4"}
,{"name": "VRApplication_VRMonitor","value": "5"}
,{"name": "VRApplication_SteamWatchdog","value": "6"}
,{"name": "VRApplication_Max","value": "7"}
,{"name": "VRApplication_Bootstrapper","value": "7"}
,{"name": "VRApplication_Max","value": "8"}
]}
, {"enumname": "vr::EVRFirmwareError","values": [
{"name": "VRFirmwareError_None","value": "0"}
@@ -559,6 +588,7 @@
,{"name": "VRApplicationProperty_IsDashboardOverlay_Bool","value": "60"}
,{"name": "VRApplicationProperty_IsTemplate_Bool","value": "61"}
,{"name": "VRApplicationProperty_IsInstanced_Bool","value": "62"}
,{"name": "VRApplicationProperty_IsInternal_Bool","value": "63"}
,{"name": "VRApplicationProperty_LastLaunchTime_Uint64","value": "70"}
]}
, {"enumname": "vr::EVRApplicationTransitionState","values": [
@@ -717,6 +747,34 @@
"constname": "k_unTrackedDeviceIndexOther","consttype": "const uint32_t", "constval": "4294967294"}
,{
"constname": "k_unTrackedDeviceIndexInvalid","consttype": "const uint32_t", "constval": "4294967295"}
,{
"constname": "k_ulInvalidPropertyContainer","consttype": "const PropertyContainerHandle_t", "constval": "0"}
,{
"constname": "k_unInvalidPropertyTag","consttype": "const PropertyTypeTag_t", "constval": "0"}
,{
"constname": "k_unFloatPropertyTag","consttype": "const PropertyTypeTag_t", "constval": "1"}
,{
"constname": "k_unInt32PropertyTag","consttype": "const PropertyTypeTag_t", "constval": "2"}
,{
"constname": "k_unUint64PropertyTag","consttype": "const PropertyTypeTag_t", "constval": "3"}
,{
"constname": "k_unBoolPropertyTag","consttype": "const PropertyTypeTag_t", "constval": "4"}
,{
"constname": "k_unStringPropertyTag","consttype": "const PropertyTypeTag_t", "constval": "5"}
,{
"constname": "k_unHmdMatrix34PropertyTag","consttype": "const PropertyTypeTag_t", "constval": "20"}
,{
"constname": "k_unHmdMatrix44PropertyTag","consttype": "const PropertyTypeTag_t", "constval": "21"}
,{
"constname": "k_unHmdVector3PropertyTag","consttype": "const PropertyTypeTag_t", "constval": "22"}
,{
"constname": "k_unHmdVector4PropertyTag","consttype": "const PropertyTypeTag_t", "constval": "23"}
,{
"constname": "k_unHiddenAreaPropertyTag","consttype": "const PropertyTypeTag_t", "constval": "30"}
,{
"constname": "k_unOpenVRInternalReserved_Start","consttype": "const PropertyTypeTag_t", "constval": "1000"}
,{
"constname": "k_unOpenVRInternalReserved_End","consttype": "const PropertyTypeTag_t", "constval": "10000"}
,{
"constname": "k_unMaxPropertyStringSize","consttype": "const uint32_t", "constval": "32768"}
,{
@@ -744,7 +802,7 @@
,{
"constname": "IVRChaperoneSetup_Version","consttype": "const char *const", "constval": "IVRChaperoneSetup_005"}
,{
"constname": "IVRCompositor_Version","consttype": "const char *const", "constval": "IVRCompositor_019"}
"constname": "IVRCompositor_Version","consttype": "const char *const", "constval": "IVRCompositor_020"}
,{
"constname": "k_unVROverlayMaxKeyLength","consttype": "const uint32_t", "constval": "128"}
,{
@@ -754,7 +812,7 @@
,{
"constname": "k_unMaxOverlayIntersectionMaskPrimitivesCount","consttype": "const uint32_t", "constval": "32"}
,{
"constname": "IVROverlay_Version","consttype": "const char *const", "constval": "IVROverlay_014"}
"constname": "IVROverlay_Version","consttype": "const char *const", "constval": "IVROverlay_016"}
,{
"constname": "k_pch_Controller_Component_GDC2015","consttype": "const char *const", "constval": "gdc2015"}
,{
@@ -787,8 +845,6 @@
"constname": "k_pch_SteamVR_DisplayDebug_Bool","consttype": "const char *const", "constval": "displayDebug"}
,{
"constname": "k_pch_SteamVR_DebugProcessPipe_String","consttype": "const char *const", "constval": "debugProcessPipe"}
,{
"constname": "k_pch_SteamVR_EnableDistortion_Bool","consttype": "const char *const", "constval": "enableDistortion"}
,{
"constname": "k_pch_SteamVR_DisplayDebugX_Int32","consttype": "const char *const", "constval": "displayDebugX"}
,{
@@ -877,8 +933,6 @@
"constname": "k_pch_Lighthouse_DBHistory_Bool","consttype": "const char *const", "constval": "dbhistory"}
,{
"constname": "k_pch_Null_Section","consttype": "const char *const", "constval": "driver_null"}
,{
"constname": "k_pch_Null_EnableNullDriver_Bool","consttype": "const char *const", "constval": "enable"}
,{
"constname": "k_pch_Null_SerialNumber_String","consttype": "const char *const", "constval": "serialNumber"}
,{
@@ -1017,6 +1071,8 @@
"constname": "k_pch_Dashboard_ArcadeMode_Bool","consttype": "const char *const", "constval": "arcadeMode"}
,{
"constname": "k_pch_modelskin_Section","consttype": "const char *const", "constval": "modelskins"}
,{
"constname": "k_pch_Driver_Enable_Bool","consttype": "const char *const", "constval": "enable"}
,{
"constname": "IVRScreenshots_Version","consttype": "const char *const", "constval": "IVRScreenshots_001"}
,{
@@ -1080,6 +1136,10 @@
{ "fieldname": "m_nHeight", "fieldtype": "uint32_t"},
{ "fieldname": "m_nFormat", "fieldtype": "uint32_t"},
{ "fieldname": "m_nSampleCount", "fieldtype": "uint32_t"}]}
,{"struct": "vr::D3D12TextureData_t","fields": [
{ "fieldname": "m_pResource", "fieldtype": "struct ID3D12Resource *"},
{ "fieldname": "m_pCommandQueue", "fieldtype": "struct ID3D12CommandQueue *"},
{ "fieldname": "m_nNodeMask", "fieldtype": "uint32_t"}]}
,{"struct": "vr::VREvent_Controller_t","fields": [
{ "fieldname": "button", "fieldtype": "uint32_t"}]}
,{"struct": "vr::VREvent_Mouse_t","fields": [
@@ -1136,6 +1196,9 @@
{ "fieldname": "nVisualMode", "fieldtype": "uint32_t"}]}
,{"struct": "vr::VREvent_MessageOverlay_t","fields": [
{ "fieldname": "unVRMessageOverlayResponse", "fieldtype": "uint32_t"}]}
,{"struct": "vr::VREvent_Property_t","fields": [
{ "fieldname": "container", "fieldtype": "PropertyContainerHandle_t"},
{ "fieldname": "prop", "fieldtype": "enum vr::ETrackedDeviceProperty"}]}
,{"struct": "vr::(anonymous)","fields": [
{ "fieldname": "reserved", "fieldtype": "struct vr::VREvent_Reserved_t"},
{ "fieldname": "controller", "fieldtype": "struct vr::VREvent_Controller_t"},
@@ -1155,7 +1218,8 @@
{ "fieldname": "screenshotProgress", "fieldtype": "struct vr::VREvent_ScreenshotProgress_t"},
{ "fieldname": "applicationLaunch", "fieldtype": "struct vr::VREvent_ApplicationLaunch_t"},
{ "fieldname": "cameraSurface", "fieldtype": "struct vr::VREvent_EditingCameraSurface_t"},
{ "fieldname": "messageOverlay", "fieldtype": "struct vr::VREvent_MessageOverlay_t"}]}
{ "fieldname": "messageOverlay", "fieldtype": "struct vr::VREvent_MessageOverlay_t"},
{ "fieldname": "property", "fieldtype": "struct vr::VREvent_Property_t"}]}
,{"struct": "vr::VREvent_t","fields": [
{ "fieldname": "eventType", "fieldtype": "uint32_t"},
{ "fieldname": "trackedDeviceIndex", "fieldtype": "TrackedDeviceIndex_t"},
@@ -2094,6 +2158,11 @@
{ "paramname": "pchWorkingDirectory" ,"paramtype": "const char *"}
]
}
,{
"classname": "vr::IVRApplications",
"methodname": "GetCurrentSceneProcessId",
"returntype": "uint32_t"
}
,{
"classname": "vr::IVRChaperone",
"methodname": "GetCalibrationState",
@@ -2559,6 +2628,14 @@
{ "paramname": "ppD3D11ShaderResourceView" ,"paramtype": "void **"}
]
}
,{
"classname": "vr::IVRCompositor",
"methodname": "ReleaseMirrorTextureD3D11",
"returntype": "void",
"params": [
{ "paramname": "pD3D11ShaderResourceView" ,"paramtype": "void *"}
]
}
,{
"classname": "vr::IVRCompositor",
"methodname": "GetMirrorTextureGL",
@@ -2628,7 +2705,7 @@
"returntype": "vr::EVROverlayError",
"params": [
{ "paramname": "pchOverlayKey" ,"paramtype": "const char *"},
{ "paramname": "pchOverlayFriendlyName" ,"paramtype": "const char *"},
{ "paramname": "pchOverlayName" ,"paramtype": "const char *"},
{ "paramname": "pOverlayHandle" ,"paramtype": "vr::VROverlayHandle_t *"}
]
}
@@ -2675,6 +2752,15 @@
{ "paramname": "pError" ,"paramtype": "vr::EVROverlayError *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "SetOverlayName",
"returntype": "vr::EVROverlayError",
"params": [
{ "paramname": "ulOverlayHandle" ,"paramtype": "vr::VROverlayHandle_t"},
{ "paramname": "pchName" ,"paramtype": "const char *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "GetOverlayImageData",
@@ -2882,6 +2968,28 @@
{ "paramname": "pOverlayTextureBounds" ,"paramtype": "struct vr::VRTextureBounds_t *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "GetOverlayRenderModel",
"returntype": "uint32_t",
"params": [
{ "paramname": "ulOverlayHandle" ,"paramtype": "vr::VROverlayHandle_t"},
{ "paramname": "pchValue" ,"paramtype": "char *"},
{ "paramname": "unBufferSize" ,"paramtype": "uint32_t"},
{ "paramname": "pColor" ,"paramtype": "struct vr::HmdColor_t *"},
{ "paramname": "pError" ,"paramtype": "vr::EVROverlayError *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "SetOverlayRenderModel",
"returntype": "vr::EVROverlayError",
"params": [
{ "paramname": "ulOverlayHandle" ,"paramtype": "vr::VROverlayHandle_t"},
{ "paramname": "pchRenderModel" ,"paramtype": "const char *"},
{ "paramname": "pColor" ,"paramtype": "const struct vr::HmdColor_t *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "GetOverlayTransformType",
@@ -2952,6 +3060,26 @@
{ "paramname": "unComponentNameSize" ,"paramtype": "uint32_t"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "GetOverlayTransformOverlayRelative",
"returntype": "vr::EVROverlayError",
"params": [
{ "paramname": "ulOverlayHandle" ,"paramtype": "vr::VROverlayHandle_t"},
{ "paramname": "ulOverlayHandleParent" ,"paramtype": "vr::VROverlayHandle_t *"},
{ "paramname": "pmatParentOverlayToOverlayTransform" ,"paramtype": "struct vr::HmdMatrix34_t *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "SetOverlayTransformOverlayRelative",
"returntype": "vr::EVROverlayError",
"params": [
{ "paramname": "ulOverlayHandle" ,"paramtype": "vr::VROverlayHandle_t"},
{ "paramname": "ulOverlayHandleParent" ,"paramtype": "vr::VROverlayHandle_t"},
{ "paramname": "pmatParentOverlayToOverlayTransform" ,"paramtype": "const struct vr::HmdMatrix34_t *"}
]
}
,{
"classname": "vr::IVROverlay",
"methodname": "ShowOverlay",

View File

@@ -17,7 +17,11 @@
#define EXTERN_C
#endif
#if defined( _WIN32 )
#define OPENVR_FNTABLE_CALLTYPE __stdcall
#else
#define OPENVR_FNTABLE_CALLTYPE
#endif
// OPENVR API export macro
#if defined( _WIN32 ) && !defined( _X360 )
@@ -50,6 +54,9 @@ typedef char bool;
#include <stdbool.h>
#endif
typedef uint64_t PropertyContainerHandle_t;
typedef uint32_t PropertyTypeTag_t;
// OpenVR Constants
@@ -58,6 +65,20 @@ static const unsigned int k_unTrackedDeviceIndex_Hmd = 0;
static const unsigned int k_unMaxTrackedDeviceCount = 16;
static const unsigned int k_unTrackedDeviceIndexOther = 4294967294;
static const unsigned int k_unTrackedDeviceIndexInvalid = 4294967295;
static const unsigned long k_ulInvalidPropertyContainer = 0;
static const unsigned int k_unInvalidPropertyTag = 0;
static const unsigned int k_unFloatPropertyTag = 1;
static const unsigned int k_unInt32PropertyTag = 2;
static const unsigned int k_unUint64PropertyTag = 3;
static const unsigned int k_unBoolPropertyTag = 4;
static const unsigned int k_unStringPropertyTag = 5;
static const unsigned int k_unHmdMatrix34PropertyTag = 20;
static const unsigned int k_unHmdMatrix44PropertyTag = 21;
static const unsigned int k_unHmdVector3PropertyTag = 22;
static const unsigned int k_unHmdVector4PropertyTag = 23;
static const unsigned int k_unHiddenAreaPropertyTag = 30;
static const unsigned int k_unOpenVRInternalReserved_Start = 1000;
static const unsigned int k_unOpenVRInternalReserved_End = 10000;
static const unsigned int k_unMaxPropertyStringSize = 32768;
static const unsigned int k_unControllerStateAxisCount = 5;
static const unsigned long k_ulOverlayHandleInvalid = 0;
@@ -71,12 +92,12 @@ static const char * k_pch_MimeType_GameTheater = "vr/game_theater";
static const char * IVRApplications_Version = "IVRApplications_006";
static const char * IVRChaperone_Version = "IVRChaperone_003";
static const char * IVRChaperoneSetup_Version = "IVRChaperoneSetup_005";
static const char * IVRCompositor_Version = "IVRCompositor_019";
static const char * IVRCompositor_Version = "IVRCompositor_020";
static const unsigned int k_unVROverlayMaxKeyLength = 128;
static const unsigned int k_unVROverlayMaxNameLength = 128;
static const unsigned int k_unMaxOverlayCount = 64;
static const unsigned int k_unMaxOverlayIntersectionMaskPrimitivesCount = 32;
static const char * IVROverlay_Version = "IVROverlay_014";
static const char * IVROverlay_Version = "IVROverlay_016";
static const char * k_pch_Controller_Component_GDC2015 = "gdc2015";
static const char * k_pch_Controller_Component_Base = "base";
static const char * k_pch_Controller_Component_Tip = "tip";
@@ -93,7 +114,6 @@ static const char * k_pch_SteamVR_ForcedDriverKey_String = "forcedDriver";
static const char * k_pch_SteamVR_ForcedHmdKey_String = "forcedHmd";
static const char * k_pch_SteamVR_DisplayDebug_Bool = "displayDebug";
static const char * k_pch_SteamVR_DebugProcessPipe_String = "debugProcessPipe";
static const char * k_pch_SteamVR_EnableDistortion_Bool = "enableDistortion";
static const char * k_pch_SteamVR_DisplayDebugX_Int32 = "displayDebugX";
static const char * k_pch_SteamVR_DisplayDebugY_Int32 = "displayDebugY";
static const char * k_pch_SteamVR_SendSystemButtonToAllApps_Bool = "sendSystemButtonToAllApps";
@@ -138,7 +158,6 @@ static const char * k_pch_Lighthouse_DisambiguationDebug_Int32 = "disambiguation
static const char * k_pch_Lighthouse_PrimaryBasestation_Int32 = "primarybasestation";
static const char * k_pch_Lighthouse_DBHistory_Bool = "dbhistory";
static const char * k_pch_Null_Section = "driver_null";
static const char * k_pch_Null_EnableNullDriver_Bool = "enable";
static const char * k_pch_Null_SerialNumber_String = "serialNumber";
static const char * k_pch_Null_ModelNumber_String = "modelNumber";
static const char * k_pch_Null_WindowX_Int32 = "windowX";
@@ -208,6 +227,7 @@ static const char * k_pch_Dashboard_Section = "dashboard";
static const char * k_pch_Dashboard_EnableDashboard_Bool = "enableDashboard";
static const char * k_pch_Dashboard_ArcadeMode_Bool = "arcadeMode";
static const char * k_pch_modelskin_Section = "modelskins";
static const char * k_pch_Driver_Enable_Bool = "enable";
static const char * IVRScreenshots_Version = "IVRScreenshots_001";
static const char * IVRResources_Version = "IVRResources_001";
@@ -224,6 +244,8 @@ typedef enum ETextureType
ETextureType_TextureType_DirectX = 0,
ETextureType_TextureType_OpenGL = 1,
ETextureType_TextureType_Vulkan = 2,
ETextureType_TextureType_IOSurface = 3,
ETextureType_TextureType_DirectX12 = 4,
} ETextureType;
typedef enum EColorSpace
@@ -249,6 +271,7 @@ typedef enum ETrackedDeviceClass
ETrackedDeviceClass_TrackedDeviceClass_Controller = 2,
ETrackedDeviceClass_TrackedDeviceClass_GenericTracker = 3,
ETrackedDeviceClass_TrackedDeviceClass_TrackingReference = 4,
ETrackedDeviceClass_TrackedDeviceClass_DisplayRedirect = 5,
} ETrackedDeviceClass;
typedef enum ETrackedControllerRole
@@ -302,6 +325,8 @@ typedef enum ETrackedDeviceProperty
ETrackedDeviceProperty_Prop_DriverVersion_String = 1031,
ETrackedDeviceProperty_Prop_Firmware_ForceUpdateRequired_Bool = 1032,
ETrackedDeviceProperty_Prop_ViveSystemButtonFixRequired_Bool = 1033,
ETrackedDeviceProperty_Prop_ParentDriver_Uint64 = 1034,
ETrackedDeviceProperty_Prop_ResourceRoot_String = 1035,
ETrackedDeviceProperty_Prop_ReportsTimeSinceVSync_Bool = 2000,
ETrackedDeviceProperty_Prop_SecondsFromVsyncToPhotons_Float = 2001,
ETrackedDeviceProperty_Prop_DisplayFrequency_Float = 2002,
@@ -340,6 +365,11 @@ typedef enum ETrackedDeviceProperty
ETrackedDeviceProperty_Prop_ScreenshotVerticalFieldOfViewDegrees_Float = 2035,
ETrackedDeviceProperty_Prop_DisplaySuppressed_Bool = 2036,
ETrackedDeviceProperty_Prop_DisplayAllowNightMode_Bool = 2037,
ETrackedDeviceProperty_Prop_DisplayMCImageWidth_Int32 = 2038,
ETrackedDeviceProperty_Prop_DisplayMCImageHeight_Int32 = 2039,
ETrackedDeviceProperty_Prop_DisplayMCImageNumChannels_Int32 = 2040,
ETrackedDeviceProperty_Prop_DisplayMCImageData_Binary = 2041,
ETrackedDeviceProperty_Prop_SecondsFromPhotonsToVblank_Float = 2042,
ETrackedDeviceProperty_Prop_AttachedDeviceId_String = 3000,
ETrackedDeviceProperty_Prop_SupportedButtons_Uint64 = 3001,
ETrackedDeviceProperty_Prop_Axis0Type_Int32 = 3002,
@@ -364,6 +394,15 @@ typedef enum ETrackedDeviceProperty
ETrackedDeviceProperty_Prop_NamedIconPathDeviceNotReady_String = 5006,
ETrackedDeviceProperty_Prop_NamedIconPathDeviceStandby_String = 5007,
ETrackedDeviceProperty_Prop_NamedIconPathDeviceAlertLow_String = 5008,
ETrackedDeviceProperty_Prop_DisplayHiddenArea_Binary_Start = 5100,
ETrackedDeviceProperty_Prop_DisplayHiddenArea_Binary_End = 5150,
ETrackedDeviceProperty_Prop_UserConfigPath_String = 6000,
ETrackedDeviceProperty_Prop_InstallPath_String = 6001,
ETrackedDeviceProperty_Prop_HasDisplayComponent_Bool = 6002,
ETrackedDeviceProperty_Prop_HasControllerComponent_Bool = 6003,
ETrackedDeviceProperty_Prop_HasCameraComponent_Bool = 6004,
ETrackedDeviceProperty_Prop_HasDriverDirectModeComponent_Bool = 6005,
ETrackedDeviceProperty_Prop_HasVirtualDisplayComponent_Bool = 6006,
ETrackedDeviceProperty_Prop_VendorSpecific_Reserved_Start = 10000,
ETrackedDeviceProperty_Prop_VendorSpecific_Reserved_End = 10999,
} ETrackedDeviceProperty;
@@ -381,6 +420,7 @@ typedef enum ETrackedPropertyError
ETrackedPropertyError_TrackedProp_StringExceedsMaximumLength = 8,
ETrackedPropertyError_TrackedProp_NotYetAvailable = 9,
ETrackedPropertyError_TrackedProp_PermissionDenied = 10,
ETrackedPropertyError_TrackedProp_InvalidOperation = 11,
} ETrackedPropertyError;
typedef enum EVRSubmitFlags
@@ -418,6 +458,7 @@ typedef enum EVREventType
EVREventType_VREvent_TrackedDeviceRoleChanged = 108,
EVREventType_VREvent_WatchdogWakeUpRequested = 109,
EVREventType_VREvent_LensDistortionChanged = 110,
EVREventType_VREvent_PropertyChanged = 111,
EVREventType_VREvent_ButtonPress = 200,
EVREventType_VREvent_ButtonUnpress = 201,
EVREventType_VREvent_ButtonTouch = 202,
@@ -464,6 +505,7 @@ typedef enum EVREventType
EVREventType_VREvent_ScreenshotFailed = 522,
EVREventType_VREvent_SubmitScreenshotToDashboard = 523,
EVREventType_VREvent_ScreenshotProgressToDashboard = 524,
EVREventType_VREvent_PrimaryDashboardDeviceChanged = 525,
EVREventType_VREvent_Notification_Shown = 600,
EVREventType_VREvent_Notification_Hidden = 601,
EVREventType_VREvent_Notification_BeginInteraction = 602,
@@ -497,6 +539,9 @@ typedef enum EVREventType
EVREventType_VREvent_ApplicationTransitionNewAppStarted = 1302,
EVREventType_VREvent_ApplicationListUpdated = 1303,
EVREventType_VREvent_ApplicationMimeTypeLoad = 1304,
EVREventType_VREvent_ApplicationTransitionNewAppLaunchComplete = 1305,
EVREventType_VREvent_ProcessConnected = 1306,
EVREventType_VREvent_ProcessDisconnected = 1307,
EVREventType_VREvent_Compositor_MirrorWindowShown = 1400,
EVREventType_VREvent_Compositor_MirrorWindowHidden = 1401,
EVREventType_VREvent_Compositor_ChaperoneBoundsShown = 1410,
@@ -557,6 +602,7 @@ typedef enum EHiddenAreaMeshType
EHiddenAreaMeshType_k_eHiddenAreaMesh_Standard = 0,
EHiddenAreaMeshType_k_eHiddenAreaMesh_Inverse = 1,
EHiddenAreaMeshType_k_eHiddenAreaMesh_LineLoop = 2,
EHiddenAreaMeshType_k_eHiddenAreaMesh_Max = 3,
} EHiddenAreaMeshType;
typedef enum EVRControllerAxisType
@@ -617,7 +663,8 @@ typedef enum EVRApplicationType
EVRApplicationType_VRApplication_Utility = 4,
EVRApplicationType_VRApplication_VRMonitor = 5,
EVRApplicationType_VRApplication_SteamWatchdog = 6,
EVRApplicationType_VRApplication_Max = 7,
EVRApplicationType_VRApplication_Bootstrapper = 7,
EVRApplicationType_VRApplication_Max = 8,
} EVRApplicationType;
typedef enum EVRFirmwareError
@@ -802,6 +849,7 @@ typedef enum EVRApplicationProperty
EVRApplicationProperty_VRApplicationProperty_IsDashboardOverlay_Bool = 60,
EVRApplicationProperty_VRApplicationProperty_IsTemplate_Bool = 61,
EVRApplicationProperty_VRApplicationProperty_IsInstanced_Bool = 62,
EVRApplicationProperty_VRApplicationProperty_IsInternal_Bool = 63,
EVRApplicationProperty_VRApplicationProperty_LastLaunchTime_Uint64 = 70,
} EVRApplicationProperty;
@@ -994,11 +1042,14 @@ typedef enum EVRScreenshotError
typedef uint32_t TrackedDeviceIndex_t;
typedef uint32_t VRNotificationId;
typedef uint64_t VROverlayHandle_t;
typedef void * glSharedTextureHandle_t;
typedef int32_t glInt_t;
typedef uint32_t glUInt_t;
typedef uint64_t SharedTextureHandle_t;
typedef uint32_t TrackedDeviceIndex_t;
typedef uint64_t PropertyContainerHandle_t;
typedef uint32_t PropertyTypeTag_t;
typedef uint64_t VROverlayHandle_t;
typedef uint64_t TrackedCameraHandle_t;
typedef uint32_t ScreenshotHandle_t;
@@ -1126,6 +1177,13 @@ typedef struct VRVulkanTextureData_t
uint32_t m_nSampleCount;
} VRVulkanTextureData_t;
typedef struct D3D12TextureData_t
{
struct ID3D12Resource * m_pResource; // struct ID3D12Resource *
struct ID3D12CommandQueue * m_pCommandQueue; // struct ID3D12CommandQueue *
uint32_t m_nNodeMask;
} D3D12TextureData_t;
typedef struct VREvent_Controller_t
{
uint32_t button;
@@ -1239,6 +1297,12 @@ typedef struct VREvent_MessageOverlay_t
uint32_t unVRMessageOverlayResponse;
} VREvent_MessageOverlay_t;
typedef struct VREvent_Property_t
{
PropertyContainerHandle_t container;
enum ETrackedDeviceProperty prop;
} VREvent_Property_t;
typedef struct HiddenAreaMesh_t
{
struct HmdVector2_t * pVertexData; // const struct vr::HmdVector2_t *
@@ -1384,6 +1448,9 @@ typedef struct RenderModel_Vertex_t
float rfTextureCoord[2]; //float[2]
} RenderModel_Vertex_t;
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( push, 4 )
#endif
typedef struct RenderModel_TextureMap_t
{
uint16_t unWidth;
@@ -1391,6 +1458,12 @@ typedef struct RenderModel_TextureMap_t
uint8_t * rubTextureMapData; // const uint8_t *
} RenderModel_TextureMap_t;
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( pop )
#endif
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( push, 4 )
#endif
typedef struct RenderModel_t
{
struct RenderModel_Vertex_t * rVertexData; // const struct vr::RenderModel_Vertex_t *
@@ -1400,6 +1473,9 @@ typedef struct RenderModel_t
TextureID_t diffuseTextureId;
} RenderModel_t;
#if defined(__linux__) || defined(__APPLE__)
#pragma pack( pop )
#endif
typedef struct RenderModel_ControllerMode_State_t
{
bool bScrollWheelVisible;
@@ -1577,6 +1653,7 @@ struct VR_IVRApplications_FnTable
char * (OPENVR_FNTABLE_CALLTYPE *GetApplicationsTransitionStateNameFromEnum)(EVRApplicationTransitionState state);
bool (OPENVR_FNTABLE_CALLTYPE *IsQuitUserPromptRequested)();
EVRApplicationError (OPENVR_FNTABLE_CALLTYPE *LaunchInternalProcess)(char * pchBinaryPath, char * pchArguments, char * pchWorkingDirectory);
uint32_t (OPENVR_FNTABLE_CALLTYPE *GetCurrentSceneProcessId)();
};
struct VR_IVRChaperone_FnTable
@@ -1651,6 +1728,7 @@ struct VR_IVRCompositor_FnTable
void (OPENVR_FNTABLE_CALLTYPE *ForceReconnectProcess)();
void (OPENVR_FNTABLE_CALLTYPE *SuspendRendering)(bool bSuspend);
EVRCompositorError (OPENVR_FNTABLE_CALLTYPE *GetMirrorTextureD3D11)(EVREye eEye, void * pD3D11DeviceOrResource, void ** ppD3D11ShaderResourceView);
void (OPENVR_FNTABLE_CALLTYPE *ReleaseMirrorTextureD3D11)(void * pD3D11ShaderResourceView);
EVRCompositorError (OPENVR_FNTABLE_CALLTYPE *GetMirrorTextureGL)(EVREye eEye, glUInt_t * pglTextureId, glSharedTextureHandle_t * pglSharedTextureHandle);
bool (OPENVR_FNTABLE_CALLTYPE *ReleaseSharedGLTexture)(glUInt_t glTextureId, glSharedTextureHandle_t glSharedTextureHandle);
void (OPENVR_FNTABLE_CALLTYPE *LockGLSharedTextureForAccess)(glSharedTextureHandle_t glSharedTextureHandle);
@@ -1662,12 +1740,13 @@ struct VR_IVRCompositor_FnTable
struct VR_IVROverlay_FnTable
{
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *FindOverlay)(char * pchOverlayKey, VROverlayHandle_t * pOverlayHandle);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *CreateOverlay)(char * pchOverlayKey, char * pchOverlayFriendlyName, VROverlayHandle_t * pOverlayHandle);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *CreateOverlay)(char * pchOverlayKey, char * pchOverlayName, VROverlayHandle_t * pOverlayHandle);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *DestroyOverlay)(VROverlayHandle_t ulOverlayHandle);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetHighQualityOverlay)(VROverlayHandle_t ulOverlayHandle);
VROverlayHandle_t (OPENVR_FNTABLE_CALLTYPE *GetHighQualityOverlay)();
uint32_t (OPENVR_FNTABLE_CALLTYPE *GetOverlayKey)(VROverlayHandle_t ulOverlayHandle, char * pchValue, uint32_t unBufferSize, EVROverlayError * pError);
uint32_t (OPENVR_FNTABLE_CALLTYPE *GetOverlayName)(VROverlayHandle_t ulOverlayHandle, char * pchValue, uint32_t unBufferSize, EVROverlayError * pError);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayName)(VROverlayHandle_t ulOverlayHandle, char * pchName);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayImageData)(VROverlayHandle_t ulOverlayHandle, void * pvBuffer, uint32_t unBufferSize, uint32_t * punWidth, uint32_t * punHeight);
char * (OPENVR_FNTABLE_CALLTYPE *GetOverlayErrorNameFromEnum)(EVROverlayError error);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayRenderingPid)(VROverlayHandle_t ulOverlayHandle, uint32_t unPID);
@@ -1690,6 +1769,8 @@ struct VR_IVROverlay_FnTable
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTextureColorSpace)(VROverlayHandle_t ulOverlayHandle, EColorSpace * peTextureColorSpace);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayTextureBounds)(VROverlayHandle_t ulOverlayHandle, struct VRTextureBounds_t * pOverlayTextureBounds);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTextureBounds)(VROverlayHandle_t ulOverlayHandle, struct VRTextureBounds_t * pOverlayTextureBounds);
uint32_t (OPENVR_FNTABLE_CALLTYPE *GetOverlayRenderModel)(VROverlayHandle_t ulOverlayHandle, char * pchValue, uint32_t unBufferSize, struct HmdColor_t * pColor, EVROverlayError * pError);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayRenderModel)(VROverlayHandle_t ulOverlayHandle, char * pchRenderModel, struct HmdColor_t * pColor);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTransformType)(VROverlayHandle_t ulOverlayHandle, VROverlayTransformType * peTransformType);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayTransformAbsolute)(VROverlayHandle_t ulOverlayHandle, ETrackingUniverseOrigin eTrackingOrigin, struct HmdMatrix34_t * pmatTrackingOriginToOverlayTransform);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTransformAbsolute)(VROverlayHandle_t ulOverlayHandle, ETrackingUniverseOrigin * peTrackingOrigin, struct HmdMatrix34_t * pmatTrackingOriginToOverlayTransform);
@@ -1697,6 +1778,8 @@ struct VR_IVROverlay_FnTable
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTransformTrackedDeviceRelative)(VROverlayHandle_t ulOverlayHandle, TrackedDeviceIndex_t * punTrackedDevice, struct HmdMatrix34_t * pmatTrackedDeviceToOverlayTransform);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayTransformTrackedDeviceComponent)(VROverlayHandle_t ulOverlayHandle, TrackedDeviceIndex_t unDeviceIndex, char * pchComponentName);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTransformTrackedDeviceComponent)(VROverlayHandle_t ulOverlayHandle, TrackedDeviceIndex_t * punDeviceIndex, char * pchComponentName, uint32_t unComponentNameSize);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *GetOverlayTransformOverlayRelative)(VROverlayHandle_t ulOverlayHandle, VROverlayHandle_t * ulOverlayHandleParent, struct HmdMatrix34_t * pmatParentOverlayToOverlayTransform);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *SetOverlayTransformOverlayRelative)(VROverlayHandle_t ulOverlayHandle, VROverlayHandle_t ulOverlayHandleParent, struct HmdMatrix34_t * pmatParentOverlayToOverlayTransform);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *ShowOverlay)(VROverlayHandle_t ulOverlayHandle);
EVROverlayError (OPENVR_FNTABLE_CALLTYPE *HideOverlay)(VROverlayHandle_t ulOverlayHandle);
bool (OPENVR_FNTABLE_CALLTYPE *IsOverlayVisible)(VROverlayHandle_t ulOverlayHandle);

File diff suppressed because it is too large Load Diff

View File

@@ -15,6 +15,7 @@
#define vsprintf_s sprintf
#define _stricmp strcmp
#define stricmp strcmp
#define strnicmp strncasecmp
#define strcpy_s(dst, n, src) int(strncpy(dst, src, n) != nullptr)
#define fopen_s(fd, path, mode) int((*fd = fopen(path, mode)) != nullptr)
#define _vsnprintf_s(buffer, size, fmt, ap) vsnprintf(buffer, size, fmt, ap)
@@ -24,4 +25,4 @@ typedef int errno_t;
#endif // _WIN32
#endif // OPENVR_SAMPLES_SHARED_COMPAT_H_
#endif // OPENVR_SAMPLES_SHARED_COMPAT_H_

View File

@@ -1,22 +1,27 @@
//========= Copyright Valve Corporation ============//
#include "compat.h"
#include "strtools.h"
#include "pathtools.h"
//#include "hmdplatform_private.h"
//#include "vrcommon/strtools.h"
#if defined( _WIN32)
#include <Windows.h>
#include <direct.h>
#include <Shobjidl.h>
#include <KnownFolders.h>
#elif defined OSX
#include <mach-o/dyld.h>
#include <dlfcn.h>
#include "osxfilebridge.h"
#define _S_IFDIR S_IFDIR // really from tier0/platform.h which we dont have yet
#define _MAX_PATH MAX_PATH // yet another form of _PATH define we use
#elif defined(LINUX)
#include <Shlobj.h>
#undef GetEnvironmentVariable
#else
#include <dlfcn.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#endif
#if defined OSX
#include <Foundation/Foundation.h>
#include <AppKit/AppKit.h>
#include <mach-o/dyld.h>
#define _S_IFDIR S_IFDIR // really from tier0/platform.h which we dont have yet
#endif
#include <sys/stat.h>
@@ -26,44 +31,54 @@
/** Returns the path (including filename) to the current executable */
std::string Path_GetExecutablePath()
{
bool bSuccess = false;
char rchPath[ 1024 ];
size_t nBuff = sizeof(rchPath);
#if defined( _WIN32 )
bSuccess = ::GetModuleFileNameA(NULL, rchPath, (DWORD)nBuff) > 0;
#elif defined OSX
uint32_t _nBuff = nBuff;
bSuccess = _NSGetExecutablePath(rchPath, &_nBuff) == 0;
rchPath[nBuff-1] = '\0';
#elif defined LINUX
ssize_t nRead = readlink("/proc/self/exe", rchPath, nBuff-1 );
if ( nRead != -1 )
{
rchPath[ nRead ] = 0;
bSuccess = true;
}
else
{
rchPath[ 0 ] = '\0';
}
#else
AssertMsg( false, "Implement Plat_GetExecutablePath" );
#endif
wchar_t *pwchPath = new wchar_t[MAX_UNICODE_PATH];
char *pchPath = new char[MAX_UNICODE_PATH_IN_UTF8];
::GetModuleFileNameW( NULL, pwchPath, MAX_UNICODE_PATH );
WideCharToMultiByte( CP_UTF8, 0, pwchPath, -1, pchPath, MAX_UNICODE_PATH_IN_UTF8, NULL, NULL );
delete[] pwchPath;
std::string sPath = pchPath;
delete[] pchPath;
return sPath;
#elif defined( OSX )
char rchPath[1024];
uint32_t nBuff = sizeof( rchPath );
bool bSuccess = _NSGetExecutablePath(rchPath, &nBuff) == 0;
rchPath[nBuff-1] = '\0';
if( bSuccess )
return rchPath;
else
return "";
#elif defined LINUX
char rchPath[1024];
size_t nBuff = sizeof( rchPath );
ssize_t nRead = readlink("/proc/self/exe", rchPath, nBuff-1 );
if ( nRead != -1 )
{
rchPath[ nRead ] = 0;
return rchPath;
}
else
{
return "";
}
#else
AssertMsg( false, "Implement Plat_GetExecutablePath" );
return "";
#endif
}
/** Returns the path of the current working directory */
std::string Path_GetWorkingDirectory()
{
std::string sPath;
char buf[ 1024 ];
#if defined( _WIN32 )
sPath = _getcwd( buf, sizeof( buf ) );
wchar_t buf[MAX_UNICODE_PATH];
sPath = UTF16to8( _wgetcwd( buf, MAX_UNICODE_PATH ) );
#else
char buf[ 1024 ];
sPath = getcwd( buf, sizeof( buf ) );
#endif
return sPath;
@@ -74,37 +89,14 @@ bool Path_SetWorkingDirectory( const std::string & sPath )
{
bool bSuccess;
#if defined( _WIN32 )
bSuccess = 0 == _chdir( sPath.c_str() );
std::wstring wsPath = UTF8to16( sPath.c_str() );
bSuccess = 0 == _wchdir( wsPath.c_str() );
#else
bSuccess = 0 == chdir( sPath.c_str() );
#endif
return bSuccess;
}
std::string Path_GetModulePath()
{
#if defined( _WIN32 )
char path[32768];
HMODULE hm = NULL;
if (!GetModuleHandleExA(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS,
(LPCSTR) &Path_GetModulePath,
&hm))
{
int ret = GetLastError();
fprintf(stderr, "GetModuleHandle returned %d\n", ret);
return "";
}
GetModuleFileNameA(hm, path, sizeof(path));
FreeLibrary( hm );
return path;
#else
Dl_info dl_info;
dladdr((void *)Path_GetModulePath, &dl_info);
return dl_info.dli_fname;
#endif
}
/** Returns the specified path without its filename */
std::string Path_StripFilename( const std::string & sPath, char slash )
{
@@ -151,17 +143,45 @@ std::string Path_StripExtension( const std::string & sPath )
return sPath;
}
/** returns just extension of the provided filename (if any). */
std::string Path_GetExtension( const std::string & sPath )
{
for ( std::string::const_reverse_iterator i = sPath.rbegin(); i != sPath.rend(); i++ )
{
if ( *i == '.' )
{
return std::string( i.base(), sPath.end() );
}
// if we find a slash there is no extension
if ( *i == '\\' || *i == '/' )
break;
}
// we didn't find an extension
return "";
}
bool Path_IsAbsolute( const std::string & sPath )
{
if( sPath.empty() )
return false;
if( sPath.find( ':' ) != std::string::npos )
return true;
#if defined( WIN32 )
if ( sPath.size() < 3 ) // must be c:\x or \\x at least
return false;
if( sPath[0] == '\\' || sPath[0] == '/' )
if ( sPath[1] == ':' ) // drive letter plus slash, but must test both slash cases
{
if ( sPath[2] == '\\' || sPath[2] == '/' )
return true;
}
else if ( sPath[0] == '\\' && sPath[1] == '\\' ) // UNC path
return true;
#else
if( sPath[0] == '\\' || sPath[0] == '/' ) // any leading slash
return true;
#endif
return false;
}
@@ -223,6 +243,8 @@ std::string Path_Join( const std::string & first, const std::string & second, ch
// only insert a slash if we don't already have one
std::string::size_type nLen = first.length();
if( !nLen )
return second;
#if defined(_WIN32)
if( first.back() == '\\' || first.back() == '/' )
nLen--;
@@ -257,6 +279,41 @@ std::string Path_Join(
return Path_Join( Path_Join( Path_Join( Path_Join( first, second, slash ), third, slash ), fourth, slash ), fifth, slash );
}
std::string Path_RemoveTrailingSlash( const std::string & sRawPath, char slash )
{
if ( slash == 0 )
slash = Path_GetSlash();
std::string sPath = sRawPath;
std::string::size_type nCurrent = sRawPath.length();
if ( nCurrent == 0 )
return sPath;
int nLastFound = -1;
nCurrent--;
while( nCurrent != 0 )
{
if ( sRawPath[ nCurrent ] == slash )
{
nLastFound = (int)nCurrent;
nCurrent--;
}
else
{
break;
}
}
if ( nLastFound >= 0 )
{
sPath.erase( nLastFound, std::string::npos );
}
return sPath;
}
/** Removes redundant <dir>/.. elements in the path. Returns an empty path if the
* specified path has a broken number of directories for its number of ..s */
std::string Path_Compact( const std::string & sRawPath, char slash )
@@ -336,17 +393,15 @@ std::string Path_Compact( const std::string & sRawPath, char slash )
return sPath;
}
#define MAX_UNICODE_PATH 32768
#define MAX_UNICODE_PATH_IN_UTF8 ( MAX_UNICODE_PATH * 4 )
/** Returns the path to the current DLL or exe */
std::string GetThisModulePath()
std::string Path_GetThisModulePath()
{
// gets the path of vrclient.dll itself
#ifdef WIN32
HMODULE hmodule = NULL;
::GetModuleHandleEx(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS | GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT, reinterpret_cast<LPCTSTR>(GetThisModulePath), &hmodule);
::GetModuleHandleEx( GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS | GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT, reinterpret_cast<LPCTSTR>(Path_GetThisModulePath), &hmodule );
wchar_t *pwchPath = new wchar_t[MAX_UNICODE_PATH];
char *pchPath = new char[ MAX_UNICODE_PATH_IN_UTF8 ];
@@ -361,7 +416,7 @@ std::string GetThisModulePath()
#elif defined( OSX ) || defined( LINUX )
// get the addr of a function in vrclient.so and then ask the dlopen system about it
Dl_info info;
dladdr( (void *)GetThisModulePath, &info );
dladdr( (void *)Path_GetThisModulePath, &info );
return info.dli_fname;
#endif
@@ -379,19 +434,44 @@ bool Path_IsDirectory( const std::string & sPath )
sFixedPath.erase( sFixedPath.end() - 1, sFixedPath.end() );
// see if the specified path actually exists.
#if defined(POSIX)
struct stat buf;
if ( stat ( sFixedPath.c_str(), &buf ) == -1)
if ( stat( sFixedPath.c_str(), &buf ) == -1 )
{
return false;
}
#if defined(LINUX)
#if defined( LINUX ) || defined( OSX )
return S_ISDIR( buf.st_mode );
#else
return ( buf.st_mode & _S_IFDIR ) != 0;
return (buf.st_mode & _S_IFDIR) != 0;
#endif
#else
struct _stat buf;
std::wstring wsFixedPath = UTF8to16( sFixedPath.c_str() );
if ( _wstat( wsFixedPath.c_str(), &buf ) == -1 )
{
return false;
}
return (buf.st_mode & _S_IFDIR) != 0;
#endif
}
/** returns true if the specified path represents an app bundle */
bool Path_IsAppBundle( const std::string & sPath )
{
#if defined(OSX)
NSBundle *bundle = [ NSBundle bundleWithPath: [ NSString stringWithUTF8String:sPath.c_str() ] ];
bool bisAppBundle = ( nullptr != bundle );
[ bundle release ];
return bisAppBundle;
#else
return false;
#endif
}
//-----------------------------------------------------------------------------
// Purpose: returns true if the the path exists
@@ -402,11 +482,20 @@ bool Path_Exists( const std::string & sPath )
if( sFixedPath.empty() )
return false;
#if defined( WIN32 )
struct _stat buf;
std::wstring wsFixedPath = UTF8to16( sFixedPath.c_str() );
if ( _wstat( wsFixedPath.c_str(), &buf ) == -1 )
{
return false;
}
#else
struct stat buf;
if ( stat ( sFixedPath.c_str(), &buf ) == -1)
{
return false;
}
#endif
return true;
}
@@ -475,11 +564,9 @@ unsigned char * Path_ReadBinaryFile( const std::string &strFilename, int *pSize
#if defined( POSIX )
f = fopen( strFilename.c_str(), "rb" );
#else
errno_t err = fopen_s(&f, strFilename.c_str(), "rb");
if ( err != 0 )
{
f = NULL;
}
std::wstring wstrFilename = UTF8to16( strFilename.c_str() );
// the open operation needs to be sharable, therefore use of _wfsopen instead of _wfopen_s
f = _wfsopen( wstrFilename.c_str(), L"rb", _SH_DENYNO );
#endif
unsigned char* buf = NULL;
@@ -508,6 +595,68 @@ unsigned char * Path_ReadBinaryFile( const std::string &strFilename, int *pSize
return buf;
}
uint32_t Path_ReadBinaryFile( const std::string &strFilename, unsigned char *pBuffer, uint32_t unSize )
{
FILE *f;
#if defined( POSIX )
f = fopen( strFilename.c_str(), "rb" );
#else
std::wstring wstrFilename = UTF8to16( strFilename.c_str() );
errno_t err = _wfopen_s( &f, wstrFilename.c_str(), L"rb" );
if ( err != 0 )
{
f = NULL;
}
#endif
uint32_t unSizeToReturn = 0;
if ( f != NULL )
{
fseek( f, 0, SEEK_END );
uint32_t size = (uint32_t)ftell( f );
fseek( f, 0, SEEK_SET );
if ( size > unSize || !pBuffer )
{
unSizeToReturn = (uint32_t)size;
}
else
{
if ( fread( pBuffer, size, 1, f ) == 1 )
{
unSizeToReturn = (uint32_t)size;
}
}
fclose( f );
}
return unSizeToReturn;
}
bool Path_WriteBinaryFile(const std::string &strFilename, unsigned char *pData, unsigned nSize)
{
FILE *f;
#if defined( POSIX )
f = fopen(strFilename.c_str(), "wb");
#else
std::wstring wstrFilename = UTF8to16( strFilename.c_str() );
errno_t err = _wfopen_s( &f, wstrFilename.c_str(), L"wb" );
if (err != 0)
{
f = NULL;
}
#endif
size_t written = 0;
if (f != NULL) {
written = fwrite(pData, sizeof(unsigned char), nSize, f);
fclose(f);
}
return written = nSize ? true : false;
}
std::string Path_ReadTextFile( const std::string &strFilename )
{
@@ -520,7 +669,7 @@ std::string Path_ReadTextFile( const std::string &strFilename )
return "";
// convert CRLF -> LF
int outsize = 1;
size_t outsize = 1;
for (int i=1; i < size; i++)
{
if (buf[i] == '\n' && buf[i-1] == '\r') // CRLF
@@ -529,7 +678,7 @@ std::string Path_ReadTextFile( const std::string &strFilename )
buf[outsize++] = buf[i]; // just copy
}
std::string ret((char *)buf, (char *)(buf + outsize));
std::string ret((char *)buf, outsize);
delete[] buf;
return ret;
}
@@ -541,7 +690,8 @@ bool Path_WriteStringToTextFile( const std::string &strFilename, const char *pch
#if defined( POSIX )
f = fopen( strFilename.c_str(), "w" );
#else
errno_t err = fopen_s(&f, strFilename.c_str(), "w");
std::wstring wstrFilename = UTF8to16( strFilename.c_str() );
errno_t err = _wfopen_s( &f, wstrFilename.c_str(), L"w" );
if ( err != 0 )
{
f = NULL;
@@ -557,4 +707,113 @@ bool Path_WriteStringToTextFile( const std::string &strFilename, const char *pch
}
return ok;
}
}
bool Path_WriteStringToTextFileAtomic( const std::string &strFilename, const char *pchData )
{
std::string strTmpFilename = strFilename + ".tmp";
if ( !Path_WriteStringToTextFile( strTmpFilename, pchData ) )
return false;
// Platform specific atomic file replacement
#if defined( _WIN32 )
std::wstring wsFilename = UTF8to16( strFilename.c_str() );
std::wstring wsTmpFilename = UTF8to16( strTmpFilename.c_str() );
if ( !::ReplaceFileW( wsFilename.c_str(), wsTmpFilename.c_str(), nullptr, 0, 0, 0 ) )
{
// if we couldn't ReplaceFile, try a non-atomic write as a fallback
if ( !Path_WriteStringToTextFile( strFilename, pchData ) )
return false;
}
#elif defined( POSIX )
if ( rename( strTmpFilename.c_str(), strFilename.c_str() ) == -1 )
return false;
#else
#error Do not know how to write atomic file
#endif
return true;
}
#if defined(WIN32)
#define FILE_URL_PREFIX "file:///"
#else
#define FILE_URL_PREFIX "file://"
#endif
// ----------------------------------------------------------------------------------------------------------------------------
// Purpose: Turns a path to a file on disk into a URL (or just returns the value if it's already a URL)
// ----------------------------------------------------------------------------------------------------------------------------
std::string Path_FilePathToUrl( const std::string & sRelativePath, const std::string & sBasePath )
{
if ( !strnicmp( sRelativePath.c_str(), "http://", 7 )
|| !strnicmp( sRelativePath.c_str(), "https://", 8 )
|| !strnicmp( sRelativePath.c_str(), "file://", 7 ) )
{
return sRelativePath;
}
else
{
std::string sAbsolute = Path_MakeAbsolute( sRelativePath, sBasePath );
if ( sAbsolute.empty() )
return sAbsolute;
return std::string( FILE_URL_PREFIX ) + sAbsolute;
}
}
// -----------------------------------------------------------------------------------------------------
// Purpose: Strips off file:// off a URL and returns the path. For other kinds of URLs an empty string is returned
// -----------------------------------------------------------------------------------------------------
std::string Path_UrlToFilePath( const std::string & sFileUrl )
{
if ( !strnicmp( sFileUrl.c_str(), FILE_URL_PREFIX, strlen( FILE_URL_PREFIX ) ) )
{
return sFileUrl.c_str() + strlen( FILE_URL_PREFIX );
}
else
{
return "";
}
}
// -----------------------------------------------------------------------------------------------------
// Purpose: Returns the root of the directory the system wants us to store user documents in
// -----------------------------------------------------------------------------------------------------
std::string GetUserDocumentsPath()
{
#if defined( WIN32 )
WCHAR rwchPath[MAX_PATH];
if ( !SUCCEEDED( SHGetFolderPathW( NULL, CSIDL_MYDOCUMENTS | CSIDL_FLAG_CREATE, NULL, 0, rwchPath ) ) )
{
return "";
}
// Convert the path to UTF-8 and store in the output
std::string sUserPath = UTF16to8( rwchPath );
return sUserPath;
#elif defined( OSX )
@autoreleasepool {
NSArray *paths = NSSearchPathForDirectoriesInDomains( NSDocumentDirectory, NSUserDomainMask, YES );
if ( [paths count] == 0 )
{
return "";
}
return [[paths objectAtIndex:0] UTF8String];
}
#elif defined( LINUX )
// @todo: not solved/changed as part of OSX - still not real - just removed old class based steam cut and paste
const char *pchHome = getenv( "HOME" );
if ( pchHome == NULL )
{
return "";
}
return pchHome;
#endif
}

View File

@@ -2,6 +2,7 @@
#pragma once
#include <string>
#include <stdint.h>
/** Returns the path (including filename) to the current executable */
std::string Path_GetExecutablePath();
@@ -13,7 +14,7 @@ std::string Path_GetWorkingDirectory();
bool Path_SetWorkingDirectory( const std::string & sPath );
/** returns the path (including filename) of the current shared lib or DLL */
std::string Path_GetModulePath();
std::string Path_GetThisModulePath();
/** Returns the specified path without its filename.
* If slash is unspecified the native path separator of the current platform
@@ -27,6 +28,9 @@ std::string Path_StripDirectory( const std::string & sPath, char slash = 0 );
* If there is a path the path is left intact. */
std::string Path_StripExtension( const std::string & sPath );
/** returns just extension of the provided filename (if any). */
std::string Path_GetExtension( const std::string & sPath );
/** Returns true if the path is absolute */
bool Path_IsAbsolute( const std::string & sPath );
@@ -60,11 +64,14 @@ std::string Path_Join(
* will be used. */
std::string Path_Compact( const std::string & sRawPath, char slash = 0 );
//** Removed trailing slashes */
std::string Path_RemoveTrailingSlash( const std::string & sRawPath, char slash = 0 );
/** returns true if the specified path exists and is a directory */
bool Path_IsDirectory( const std::string & sPath );
/** Returns the path to the current DLL or exe */
std::string GetThisModulePath();
/** returns true if the specified path represents an app bundle */
bool Path_IsAppBundle( const std::string & sPath );
/** returns true if the the path exists */
bool Path_Exists( const std::string & sPath );
@@ -75,11 +82,31 @@ std::string Path_FindParentSubDirectoryRecursively( const std::string &strStartD
/** Path operations to read or write text/binary files */
unsigned char * Path_ReadBinaryFile( const std::string &strFilename, int *pSize );
uint32_t Path_ReadBinaryFile( const std::string &strFilename, unsigned char *pBuffer, uint32_t unSize );
bool Path_WriteBinaryFile( const std::string &strFilename, unsigned char *pData, unsigned nSize );
std::string Path_ReadTextFile( const std::string &strFilename );
bool Path_WriteStringToTextFile( const std::string &strFilename, const char *pchData );
bool Path_WriteStringToTextFileAtomic( const std::string &strFilename, const char *pchData );
/** Returns a file:// url for paths, or an http or https url if that's what was provided */
std::string Path_FilePathToUrl( const std::string & sRelativePath, const std::string & sBasePath );
/** Strips off file:// off a URL and returns the path. For other kinds of URLs an empty string is returned */
std::string Path_UrlToFilePath( const std::string & sFileUrl );
/** Returns the root of the directory the system wants us to store user documents in */
std::string GetUserDocumentsPath();
#ifndef MAX_UNICODE_PATH
#define MAX_UNICODE_PATH 32767
#endif
#ifndef MAX_UNICODE_PATH_IN_UTF8
#define MAX_UNICODE_PATH_IN_UTF8 (MAX_UNICODE_PATH * 4)
#endif
//-----------------------------------------------------------------------------
#if defined(_WIN32)
#if defined(WIN32)
#define DYNAMIC_LIB_EXT ".dll"
#ifdef _WIN64
#define PLATSUBDIR "win64"
@@ -91,8 +118,12 @@ bool Path_WriteStringToTextFile( const std::string &strFilename, const char *pch
#define PLATSUBDIR "osx32"
#elif defined(LINUX)
#define DYNAMIC_LIB_EXT ".so"
#if defined( LINUX32 )
#define PLATSUBDIR "linux32"
#else
#define PLATSUBDIR "linux64"
#endif
#else
#warning "Unknown platform for PLATSUBDIR"
#define PLATSUBDIR "unknown_platform"
#endif

View File

@@ -0,0 +1,437 @@
//========= Copyright Valve Corporation ============//
#include "strtools.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
bool StringHasPrefix( const std::string & sString, const std::string & sPrefix )
{
return 0 == strnicmp( sString.c_str(), sPrefix.c_str(), sPrefix.length() );
}
bool StringHasPrefixCaseSensitive( const std::string & sString, const std::string & sPrefix )
{
return 0 == strncmp( sString.c_str(), sPrefix.c_str(), sPrefix.length() );
}
bool StringHasSuffix( const std::string &sString, const std::string &sSuffix )
{
size_t cStrLen = sString.length();
size_t cSuffixLen = sSuffix.length();
if ( cSuffixLen > cStrLen )
return false;
std::string sStringSuffix = sString.substr( cStrLen - cSuffixLen, cSuffixLen );
return 0 == stricmp( sStringSuffix.c_str(), sSuffix.c_str() );
}
bool StringHasSuffixCaseSensitive( const std::string &sString, const std::string &sSuffix )
{
size_t cStrLen = sString.length();
size_t cSuffixLen = sSuffix.length();
if ( cSuffixLen > cStrLen )
return false;
std::string sStringSuffix = sString.substr( cStrLen - cSuffixLen, cSuffixLen );
return 0 == strncmp( sStringSuffix.c_str(), sSuffix.c_str(),cSuffixLen );
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
std::string UTF16to8(const wchar_t * in)
{
std::string out;
unsigned int codepoint = 0;
for ( ; in && *in != 0; ++in )
{
if (*in >= 0xd800 && *in <= 0xdbff)
codepoint = ((*in - 0xd800) << 10) + 0x10000;
else
{
if (*in >= 0xdc00 && *in <= 0xdfff)
codepoint |= *in - 0xdc00;
else
codepoint = *in;
if (codepoint <= 0x7f)
out.append(1, static_cast<char>(codepoint));
else if (codepoint <= 0x7ff)
{
out.append(1, static_cast<char>(0xc0 | ((codepoint >> 6) & 0x1f)));
out.append(1, static_cast<char>(0x80 | (codepoint & 0x3f)));
}
else if (codepoint <= 0xffff)
{
out.append(1, static_cast<char>(0xe0 | ((codepoint >> 12) & 0x0f)));
out.append(1, static_cast<char>(0x80 | ((codepoint >> 6) & 0x3f)));
out.append(1, static_cast<char>(0x80 | (codepoint & 0x3f)));
}
else
{
out.append(1, static_cast<char>(0xf0 | ((codepoint >> 18) & 0x07)));
out.append(1, static_cast<char>(0x80 | ((codepoint >> 12) & 0x3f)));
out.append(1, static_cast<char>(0x80 | ((codepoint >> 6) & 0x3f)));
out.append(1, static_cast<char>(0x80 | (codepoint & 0x3f)));
}
codepoint = 0;
}
}
return out;
}
std::wstring UTF8to16(const char * in)
{
std::wstring out;
unsigned int codepoint = 0;
int following = 0;
for ( ; in && *in != 0; ++in )
{
unsigned char ch = *in;
if (ch <= 0x7f)
{
codepoint = ch;
following = 0;
}
else if (ch <= 0xbf)
{
if (following > 0)
{
codepoint = (codepoint << 6) | (ch & 0x3f);
--following;
}
}
else if (ch <= 0xdf)
{
codepoint = ch & 0x1f;
following = 1;
}
else if (ch <= 0xef)
{
codepoint = ch & 0x0f;
following = 2;
}
else
{
codepoint = ch & 0x07;
following = 3;
}
if (following == 0)
{
if (codepoint > 0xffff)
{
out.append(1, static_cast<wchar_t>(0xd800 + (codepoint >> 10)));
out.append(1, static_cast<wchar_t>(0xdc00 + (codepoint & 0x03ff)));
}
else
out.append(1, static_cast<wchar_t>(codepoint));
codepoint = 0;
}
}
return out;
}
void strcpy_safe( char *pchBuffer, size_t unBufferSizeBytes, const char *pchSource )
{
pchBuffer[ unBufferSizeBytes - 1 ] = '\0';
strncpy( pchBuffer, pchSource, unBufferSizeBytes - 1 );
}
// --------------------------------------------------------------------
// Purpose: converts a string to upper case
// --------------------------------------------------------------------
std::string StringToUpper( const std::string & sString )
{
std::string sOut;
sOut.reserve( sString.size() + 1 );
for( std::string::const_iterator i = sString.begin(); i != sString.end(); i++ )
{
sOut.push_back( (char)toupper( *i ) );
}
return sOut;
}
// --------------------------------------------------------------------
// Purpose: converts a string to lower case
// --------------------------------------------------------------------
std::string StringToLower( const std::string & sString )
{
std::string sOut;
sOut.reserve( sString.size() + 1 );
for( std::string::const_iterator i = sString.begin(); i != sString.end(); i++ )
{
sOut.push_back( (char)tolower( *i ) );
}
return sOut;
}
uint32_t ReturnStdString( const std::string & sValue, char *pchBuffer, uint32_t unBufferLen )
{
uint32_t unLen = (uint32_t)sValue.length() + 1;
if( !pchBuffer || !unBufferLen )
return unLen;
if( unBufferLen < unLen )
{
pchBuffer[0] = '\0';
}
else
{
memcpy( pchBuffer, sValue.c_str(), unLen );
}
return unLen;
}
void BufferToStdString( std::string & sDest, const char *pchBuffer, uint32_t unBufferLen )
{
sDest.resize( unBufferLen + 1 );
memcpy( const_cast< char* >( sDest.c_str() ), pchBuffer, unBufferLen );
const_cast< char* >( sDest.c_str() )[ unBufferLen ] = '\0';
}
/** Returns a std::string from a uint64_t */
std::string Uint64ToString( uint64_t ulValue )
{
char buf[ 22 ];
#if defined( _WIN32 )
sprintf_s( buf, "%llu", ulValue );
#else
snprintf( buf, sizeof( buf ), "%llu", (long long unsigned int ) ulValue );
#endif
return buf;
}
/** returns a uint64_t from a string */
uint64_t StringToUint64( const std::string & sValue )
{
return strtoull( sValue.c_str(), NULL, 0 );
}
//-----------------------------------------------------------------------------
// Purpose: Helper for converting a numeric value to a hex digit, value should be 0-15.
//-----------------------------------------------------------------------------
char cIntToHexDigit( int nValue )
{
//Assert( nValue >= 0 && nValue <= 15 );
return "0123456789ABCDEF"[ nValue & 15 ];
}
//-----------------------------------------------------------------------------
// Purpose: Helper for converting a hex char value to numeric, return -1 if the char
// is not a valid hex digit.
//-----------------------------------------------------------------------------
int iHexCharToInt( char cValue )
{
int32_t iValue = cValue;
if ( (uint32_t)( iValue - '0' ) < 10 )
return iValue - '0';
iValue |= 0x20;
if ( (uint32_t)( iValue - 'a' ) < 6 )
return iValue - 'a' + 10;
return -1;
}
//-----------------------------------------------------------------------------
// Purpose: Internal implementation of encode, works in the strict RFC manner, or
// with spaces turned to + like HTML form encoding.
//-----------------------------------------------------------------------------
void V_URLEncodeInternal( char *pchDest, int nDestLen, const char *pchSource, int nSourceLen, bool bUsePlusForSpace )
{
//AssertMsg( nDestLen > 3*nSourceLen, "Target buffer for V_URLEncode should be 3x source length, plus one for terminating null\n" );
int iDestPos = 0;
for ( int i=0; i < nSourceLen; ++i )
{
// worst case we need 3 additional chars
if( (iDestPos+3) > nDestLen )
{
pchDest[0] = '\0';
// AssertMsg( false, "Target buffer too short\n" );
return;
}
// We allow only a-z, A-Z, 0-9, period, underscore, and hyphen to pass through unescaped.
// These are the characters allowed by both the original RFC 1738 and the latest RFC 3986.
// Current specs also allow '~', but that is forbidden under original RFC 1738.
if ( !( pchSource[i] >= 'a' && pchSource[i] <= 'z' ) && !( pchSource[i] >= 'A' && pchSource[i] <= 'Z' ) && !(pchSource[i] >= '0' && pchSource[i] <= '9' )
&& pchSource[i] != '-' && pchSource[i] != '_' && pchSource[i] != '.'
)
{
if ( bUsePlusForSpace && pchSource[i] == ' ' )
{
pchDest[iDestPos++] = '+';
}
else
{
pchDest[iDestPos++] = '%';
uint8_t iValue = pchSource[i];
if ( iValue == 0 )
{
pchDest[iDestPos++] = '0';
pchDest[iDestPos++] = '0';
}
else
{
char cHexDigit1 = cIntToHexDigit( iValue % 16 );
iValue /= 16;
char cHexDigit2 = cIntToHexDigit( iValue );
pchDest[iDestPos++] = cHexDigit2;
pchDest[iDestPos++] = cHexDigit1;
}
}
}
else
{
pchDest[iDestPos++] = pchSource[i];
}
}
if( (iDestPos+1) > nDestLen )
{
pchDest[0] = '\0';
//AssertMsg( false, "Target buffer too short to terminate\n" );
return;
}
// Null terminate
pchDest[iDestPos++] = 0;
}
//-----------------------------------------------------------------------------
// Purpose: Internal implementation of decode, works in the strict RFC manner, or
// with spaces turned to + like HTML form encoding.
//
// Returns the amount of space used in the output buffer.
//-----------------------------------------------------------------------------
size_t V_URLDecodeInternal( char *pchDecodeDest, int nDecodeDestLen, const char *pchEncodedSource, int nEncodedSourceLen, bool bUsePlusForSpace )
{
if ( nDecodeDestLen < nEncodedSourceLen )
{
//AssertMsg( false, "V_URLDecode needs a dest buffer at least as large as the source" );
return 0;
}
int iDestPos = 0;
for( int i=0; i < nEncodedSourceLen; ++i )
{
if ( bUsePlusForSpace && pchEncodedSource[i] == '+' )
{
pchDecodeDest[ iDestPos++ ] = ' ';
}
else if ( pchEncodedSource[i] == '%' )
{
// Percent signifies an encoded value, look ahead for the hex code, convert to numeric, and use that
// First make sure we have 2 more chars
if ( i < nEncodedSourceLen - 2 )
{
char cHexDigit1 = pchEncodedSource[i+1];
char cHexDigit2 = pchEncodedSource[i+2];
// Turn the chars into a hex value, if they are not valid, then we'll
// just place the % and the following two chars direct into the string,
// even though this really shouldn't happen, who knows what bad clients
// may do with encoding.
bool bValid = false;
int iValue = iHexCharToInt( cHexDigit1 );
if ( iValue != -1 )
{
iValue *= 16;
int iValue2 = iHexCharToInt( cHexDigit2 );
if ( iValue2 != -1 )
{
iValue += iValue2;
pchDecodeDest[ iDestPos++ ] = (char)iValue;
bValid = true;
}
}
if ( !bValid )
{
pchDecodeDest[ iDestPos++ ] = '%';
pchDecodeDest[ iDestPos++ ] = cHexDigit1;
pchDecodeDest[ iDestPos++ ] = cHexDigit2;
}
}
// Skip ahead
i += 2;
}
else
{
pchDecodeDest[ iDestPos++ ] = pchEncodedSource[i];
}
}
// We may not have extra room to NULL terminate, since this can be used on raw data, but if we do
// go ahead and do it as this can avoid bugs.
if ( iDestPos < nDecodeDestLen )
{
pchDecodeDest[iDestPos] = 0;
}
return (size_t)iDestPos;
}
//-----------------------------------------------------------------------------
// Purpose: Encodes a string (or binary data) from URL encoding format, see rfc1738 section 2.2.
// This version of the call isn't a strict RFC implementation, but uses + for space as is
// the standard in HTML form encoding, despite it not being part of the RFC.
//
// Dest buffer should be at least as large as source buffer to guarantee room for decode.
//-----------------------------------------------------------------------------
void V_URLEncode( char *pchDest, int nDestLen, const char *pchSource, int nSourceLen )
{
return V_URLEncodeInternal( pchDest, nDestLen, pchSource, nSourceLen, true );
}
//-----------------------------------------------------------------------------
// Purpose: Decodes a string (or binary data) from URL encoding format, see rfc1738 section 2.2.
// This version of the call isn't a strict RFC implementation, but uses + for space as is
// the standard in HTML form encoding, despite it not being part of the RFC.
//
// Dest buffer should be at least as large as source buffer to guarantee room for decode.
// Dest buffer being the same as the source buffer (decode in-place) is explicitly allowed.
//-----------------------------------------------------------------------------
size_t V_URLDecode( char *pchDecodeDest, int nDecodeDestLen, const char *pchEncodedSource, int nEncodedSourceLen )
{
return V_URLDecodeInternal( pchDecodeDest, nDecodeDestLen, pchEncodedSource, nEncodedSourceLen, true );
}
//-----------------------------------------------------------------------------
void V_StripExtension( std::string &in )
{
// Find the last dot. If it's followed by a dot or a slash, then it's part of a
// directory specifier like ../../somedir/./blah.
std::string::size_type test = in.rfind( '.' );
if ( test != std::string::npos )
{
// This handles things like ".\blah" or "c:\my@email.com\abc\def\geh"
// Which would otherwise wind up with "" and "c:\my@email", respectively.
if ( in.rfind( '\\' ) < test && in.rfind( '/' ) < test )
{
in.resize( test );
}
}
}

View File

@@ -0,0 +1,130 @@
//========= Copyright Valve Corporation ============//
#pragma once
#include <string>
#include <stdint.h>
#include <sys/types.h>
/** returns true if the string has the prefix */
bool StringHasPrefix( const std::string & sString, const std::string & sPrefix );
bool StringHasPrefixCaseSensitive( const std::string & sString, const std::string & sPrefix );
/** returns if the string has the suffix */
bool StringHasSuffix( const std::string &sString, const std::string &sSuffix );
bool StringHasSuffixCaseSensitive( const std::string &sString, const std::string &sSuffix );
/** converts a UTF-16 string to a UTF-8 string */
std::string UTF16to8(const wchar_t * in);
/** converts a UTF-8 string to a UTF-16 string */
std::wstring UTF8to16(const char * in);
#define Utf16FromUtf8 UTF8to16
/** safely copy a string into a buffer */
void strcpy_safe( char *pchBuffer, size_t unBufferSizeBytes, const char *pchSource );
template< size_t bufferSize >
void strcpy_safe( char (& buffer) [ bufferSize ], const char *pchSource )
{
strcpy_safe( buffer, bufferSize, pchSource );
}
/** converts a string to upper case */
std::string StringToUpper( const std::string & sString );
/** converts a string to lower case */
std::string StringToLower( const std::string & sString );
// we stricmp (from WIN) but it isn't POSIX - OSX/LINUX have strcasecmp so just inline bridge to it
#if defined( OSX ) || defined( LINUX )
#include <strings.h>
inline int stricmp(const char *pStr1, const char *pStr2) { return strcasecmp(pStr1,pStr2); }
#ifndef _stricmp
#define _stricmp stricmp
#endif
inline int strnicmp( const char *pStr1, const char *pStr2, size_t unBufferLen ) { return strncasecmp( pStr1,pStr2, unBufferLen ); }
#define _strnicmp strnicmp
#ifndef _vsnprintf_s
#define _vsnprintf_s vsnprintf
#endif
#define _TRUNCATE ((size_t)-1)
#endif
#if defined( OSX )
// behaviors ensure NULL-termination at least as well as _TRUNCATE does, but
// wcsncpy_s/strncpy_s can non-NULL-terminate, wcslcpy/strlcpy can not.
inline errno_t wcsncpy_s(wchar_t *strDest, size_t numberOfElements, const wchar_t *strSource, size_t count)
{
return wcslcpy(strDest, strSource, numberOfElements);
}
inline errno_t strncpy_s(char *strDest, size_t numberOfElements, const char *strSource, size_t count)
{
return strlcpy(strDest, strSource, numberOfElements);
}
#endif
#if defined( LINUX )
// this implementation does not return whether or not the destination was
// truncated, but that is straightforward to fix if anybody actually needs the
// return code.
#include "string.h"
inline void wcsncpy_s(wchar_t *strDest, size_t numberOfElements, const wchar_t *strSource, size_t count)
{
wcsncpy(strDest, strSource, numberOfElements);
strDest[numberOfElements-1] = '\0';
}
inline void strncpy_s(char *strDest, size_t numberOfElements, const char *strSource, size_t count)
{
strncpy(strDest, strSource, numberOfElements);
strDest[numberOfElements-1] = '\0';
}
#endif
#if defined( _WIN32 ) && _MSC_VER < 1800
inline uint64_t strtoull(const char *str, char **endptr, int base) { return _strtoui64( str, endptr, base ); }
#endif
/* Handles copying a std::string into a buffer as would be provided in an API */
uint32_t ReturnStdString( const std::string & sValue, char *pchBuffer, uint32_t unBufferLen );
/* Handles copying a buffer into an std::string and auto adds null terminator */
void BufferToStdString( std::string & sDest, const char *pchBuffer, uint32_t unBufferLen );
/** Returns a std::string from a uint64_t */
std::string Uint64ToString( uint64_t ulValue );
/** returns a uint64_t from a string */
uint64_t StringToUint64( const std::string & sValue );
//-----------------------------------------------------------------------------
// Purpose: Encodes a string (or binary data) from URL encoding format, see rfc1738 section 2.2.
// This version of the call isn't a strict RFC implementation, but uses + for space as is
// the standard in HTML form encoding, despite it not being part of the RFC.
//
// Dest buffer should be at least as large as source buffer to guarantee room for decode.
//-----------------------------------------------------------------------------
void V_URLEncode( char *pchDest, int nDestLen, const char *pchSource, int nSourceLen );
//-----------------------------------------------------------------------------
// Purpose: Decodes a string (or binary data) from URL encoding format, see rfc1738 section 2.2.
// This version of the call isn't a strict RFC implementation, but uses + for space as is
// the standard in HTML form encoding, despite it not being part of the RFC.
//
// Dest buffer should be at least as large as source buffer to guarantee room for decode.
// Dest buffer being the same as the source buffer (decode in-place) is explicitly allowed.
//-----------------------------------------------------------------------------
size_t V_URLDecode( char *pchDecodeDest, int nDecodeDestLen, const char *pchEncodedSource, int nEncodedSourceLen );
//-----------------------------------------------------------------------------
// Purpose: strip extension from a path
//-----------------------------------------------------------------------------
void V_StripExtension( std::string &in );

View File

@@ -0,0 +1,78 @@
# Serial Communication Library
[![Build Status](https://travis-ci.org/wjwwood/serial.svg?branch=master)](https://travis-ci.org/wjwwood/serial)*(Linux and OS X)* [![Build Status](https://ci.appveyor.com/api/projects/status/github/wjwwood/serial)](https://ci.appveyor.com/project/wjwwood/serial)*(Windows)*
This is a cross-platform library for interfacing with rs-232 serial like ports written in C++. It provides a modern C++ interface with a workflow designed to look and feel like PySerial, but with the speed and control provided by C++.
This library is in use in several robotics related projects and can be built and installed to the OS like most unix libraries with make and then sudo make install, but because it is a catkin project it can also be built along side other catkin projects in a catkin workspace.
Serial is a class that provides the basic interface common to serial libraries (open, close, read, write, etc..) and requires no extra dependencies. It also provides tight control over timeouts and control over handshaking lines.
### Documentation
Website: http://wjwwood.github.com/serial/
API Documentation: http://wjwwood.github.com/serial/doc/1.1.0/index.html
### Dependencies
Required:
* [catkin](http://www.ros.org/wiki/catkin) - cmake and Python based buildsystem
* [cmake](http://www.cmake.org) - buildsystem
* [Python](http://www.python.org) - scripting language
* [empy](http://www.alcyone.com/pyos/empy/) - Python templating library
* [catkin_pkg](http://pypi.python.org/pypi/catkin_pkg/) - Runtime Python library for catkin
Optional (for tests):
* [Boost](http://www.boost.org/) - Boost C++ librairies
Optional (for documentation):
* [Doxygen](http://www.doxygen.org/) - Documentation generation tool
* [graphviz](http://www.graphviz.org/) - Graph visualization software
### Install
Get the code:
git clone https://github.com/wjwwood/serial.git
Build:
make
Build and run the tests:
make test
Build the documentation:
make doc
Install:
make install
Uninstall:
make uninstall
### License
The MIT License
Copyright (c) 2012 William Woodall, John Harrison
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
### Authors
William Woodall <wjwwood@gmail.com>
John Harrison <ash.gti@gmail.com>
### Contact
William Woodall <william@osrfoundation.org>

View File

@@ -0,0 +1,221 @@
/*!
* \file serial/impl/unix.h
* \author William Woodall <wjwwood@gmail.com>
* \author John Harrison <ash@greaterthaninfinity.com>
* \version 0.1
*
* \section LICENSE
*
* The MIT License
*
* Copyright (c) 2012 William Woodall, John Harrison
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*
* \section DESCRIPTION
*
* This provides a unix based pimpl for the Serial class. This implementation is
* based off termios.h and uses select for multiplexing the IO ports.
*
*/
#if !defined(_WIN32)
#ifndef SERIAL_IMPL_UNIX_H
#define SERIAL_IMPL_UNIX_H
#include "serial/serial.h"
#include <pthread.h>
namespace serial {
using std::size_t;
using std::string;
using std::invalid_argument;
using serial::SerialException;
using serial::IOException;
class MillisecondTimer {
public:
MillisecondTimer(const uint32_t millis);
int64_t remaining();
private:
static timespec timespec_now();
timespec expiry;
};
class serial::Serial::SerialImpl {
public:
SerialImpl (const string &port,
unsigned long baudrate,
bytesize_t bytesize,
parity_t parity,
stopbits_t stopbits,
flowcontrol_t flowcontrol);
virtual ~SerialImpl ();
void
open ();
void
close ();
bool
isOpen () const;
size_t
available ();
bool
waitReadable (uint32_t timeout);
void
waitByteTimes (size_t count);
size_t
read (uint8_t *buf, size_t size = 1);
size_t
write (const uint8_t *data, size_t length);
void
flush ();
void
flushInput ();
void
flushOutput ();
void
sendBreak (int duration);
void
setBreak (bool level);
void
setRTS (bool level);
void
setDTR (bool level);
bool
waitForChange ();
bool
getCTS ();
bool
getDSR ();
bool
getRI ();
bool
getCD ();
void
setPort (const string &port);
string
getPort () const;
void
setTimeout (Timeout &timeout);
Timeout
getTimeout () const;
void
setBaudrate (unsigned long baudrate);
unsigned long
getBaudrate () const;
void
setBytesize (bytesize_t bytesize);
bytesize_t
getBytesize () const;
void
setParity (parity_t parity);
parity_t
getParity () const;
void
setStopbits (stopbits_t stopbits);
stopbits_t
getStopbits () const;
void
setFlowcontrol (flowcontrol_t flowcontrol);
flowcontrol_t
getFlowcontrol () const;
void
readLock ();
void
readUnlock ();
void
writeLock ();
void
writeUnlock ();
protected:
void reconfigurePort ();
private:
string port_; // Path to the file descriptor
int fd_; // The current file descriptor
bool is_open_;
bool xonxoff_;
bool rtscts_;
Timeout timeout_; // Timeout for read operations
unsigned long baudrate_; // Baudrate
uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte
parity_t parity_; // Parity
bytesize_t bytesize_; // Size of the bytes
stopbits_t stopbits_; // Stop Bits
flowcontrol_t flowcontrol_; // Flow Control
// Mutex used to lock the read functions
pthread_mutex_t read_mutex;
// Mutex used to lock the write functions
pthread_mutex_t write_mutex;
};
}
#endif // SERIAL_IMPL_UNIX_H
#endif // !defined(_WIN32)

View File

@@ -0,0 +1,207 @@
/*!
* \file serial/impl/windows.h
* \author William Woodall <wjwwood@gmail.com>
* \author John Harrison <ash@greaterthaninfinity.com>
* \version 0.1
*
* \section LICENSE
*
* The MIT License
*
* Copyright (c) 2012 William Woodall, John Harrison
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*
* \section DESCRIPTION
*
* This provides a windows implementation of the Serial class interface.
*
*/
#if defined(_WIN32)
#ifndef SERIAL_IMPL_WINDOWS_H
#define SERIAL_IMPL_WINDOWS_H
#include "serial/serial.h"
#include "windows.h"
namespace serial {
using std::string;
using std::wstring;
using std::invalid_argument;
using serial::SerialException;
using serial::IOException;
class serial::Serial::SerialImpl {
public:
SerialImpl (const string &port,
unsigned long baudrate,
bytesize_t bytesize,
parity_t parity,
stopbits_t stopbits,
flowcontrol_t flowcontrol);
virtual ~SerialImpl ();
void
open ();
void
close ();
bool
isOpen () const;
size_t
available ();
bool
waitReadable (uint32_t timeout);
void
waitByteTimes (size_t count);
size_t
read (uint8_t *buf, size_t size = 1);
size_t
write (const uint8_t *data, size_t length);
void
flush ();
void
flushInput ();
void
flushOutput ();
void
sendBreak (int duration);
void
setBreak (bool level);
void
setRTS (bool level);
void
setDTR (bool level);
bool
waitForChange ();
bool
getCTS ();
bool
getDSR ();
bool
getRI ();
bool
getCD ();
void
setPort (const string &port);
string
getPort () const;
void
setTimeout (Timeout &timeout);
Timeout
getTimeout () const;
void
setBaudrate (unsigned long baudrate);
unsigned long
getBaudrate () const;
void
setBytesize (bytesize_t bytesize);
bytesize_t
getBytesize () const;
void
setParity (parity_t parity);
parity_t
getParity () const;
void
setStopbits (stopbits_t stopbits);
stopbits_t
getStopbits () const;
void
setFlowcontrol (flowcontrol_t flowcontrol);
flowcontrol_t
getFlowcontrol () const;
void
readLock ();
void
readUnlock ();
void
writeLock ();
void
writeUnlock ();
protected:
void reconfigurePort ();
private:
wstring port_; // Path to the file descriptor
HANDLE fd_;
bool is_open_;
Timeout timeout_; // Timeout for read operations
unsigned long baudrate_; // Baudrate
parity_t parity_; // Parity
bytesize_t bytesize_; // Size of the bytes
stopbits_t stopbits_; // Stop Bits
flowcontrol_t flowcontrol_; // Flow Control
// Mutex used to lock the read functions
HANDLE read_mutex;
// Mutex used to lock the write functions
HANDLE write_mutex;
};
}
#endif // SERIAL_IMPL_WINDOWS_H
#endif // if defined(_WIN32)

View File

@@ -0,0 +1,775 @@
/*!
* \file serial/serial.h
* \author William Woodall <wjwwood@gmail.com>
* \author John Harrison <ash.gti@gmail.com>
* \version 0.1
*
* \section LICENSE
*
* The MIT License
*
* Copyright (c) 2012 William Woodall
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*
* \section DESCRIPTION
*
* This provides a cross platform interface for interacting with Serial Ports.
*/
#ifndef SERIAL_H
#define SERIAL_H
#include <limits>
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <exception>
#include <stdexcept>
#include <serial/v8stdint.h>
#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
__LINE__, (message) )
namespace serial {
/*!
* Enumeration defines the possible bytesizes for the serial port.
*/
typedef enum {
fivebits = 5,
sixbits = 6,
sevenbits = 7,
eightbits = 8
} bytesize_t;
/*!
* Enumeration defines the possible parity types for the serial port.
*/
typedef enum {
parity_none = 0,
parity_odd = 1,
parity_even = 2,
parity_mark = 3,
parity_space = 4
} parity_t;
/*!
* Enumeration defines the possible stopbit types for the serial port.
*/
typedef enum {
stopbits_one = 1,
stopbits_two = 2,
stopbits_one_point_five
} stopbits_t;
/*!
* Enumeration defines the possible flowcontrol types for the serial port.
*/
typedef enum {
flowcontrol_none = 0,
flowcontrol_software,
flowcontrol_hardware
} flowcontrol_t;
/*!
* Structure for setting the timeout of the serial port, times are
* in milliseconds.
*
* In order to disable the interbyte timeout, set it to Timeout::max().
*/
struct Timeout {
#ifdef max
# undef max
#endif
static uint32_t max() {return std::numeric_limits<uint32_t>::max();}
/*!
* Convenience function to generate Timeout structs using a
* single absolute timeout.
*
* \param timeout A long that defines the time in milliseconds until a
* timeout occurs after a call to read or write is made.
*
* \return Timeout struct that represents this simple timeout provided.
*/
static Timeout simpleTimeout(uint32_t timeout) {
return Timeout(max(), timeout, 0, timeout, 0);
}
/*! Number of milliseconds between bytes received to timeout on. */
uint32_t inter_byte_timeout;
/*! A constant number of milliseconds to wait after calling read. */
uint32_t read_timeout_constant;
/*! A multiplier against the number of requested bytes to wait after
* calling read.
*/
uint32_t read_timeout_multiplier;
/*! A constant number of milliseconds to wait after calling write. */
uint32_t write_timeout_constant;
/*! A multiplier against the number of requested bytes to wait after
* calling write.
*/
uint32_t write_timeout_multiplier;
explicit Timeout (uint32_t inter_byte_timeout_=0,
uint32_t read_timeout_constant_=0,
uint32_t read_timeout_multiplier_=0,
uint32_t write_timeout_constant_=0,
uint32_t write_timeout_multiplier_=0)
: inter_byte_timeout(inter_byte_timeout_),
read_timeout_constant(read_timeout_constant_),
read_timeout_multiplier(read_timeout_multiplier_),
write_timeout_constant(write_timeout_constant_),
write_timeout_multiplier(write_timeout_multiplier_)
{}
};
/*!
* Class that provides a portable serial port interface.
*/
class Serial {
public:
/*!
* Creates a Serial object and opens the port if a port is specified,
* otherwise it remains closed until serial::Serial::open is called.
*
* \param port A std::string containing the address of the serial port,
* which would be something like 'COM1' on Windows and '/dev/ttyS0'
* on Linux.
*
* \param baudrate An unsigned 32-bit integer that represents the baudrate
*
* \param timeout A serial::Timeout struct that defines the timeout
* conditions for the serial port. \see serial::Timeout
*
* \param bytesize Size of each byte in the serial transmission of data,
* default is eightbits, possible values are: fivebits, sixbits, sevenbits,
* eightbits
*
* \param parity Method of parity, default is parity_none, possible values
* are: parity_none, parity_odd, parity_even
*
* \param stopbits Number of stop bits used, default is stopbits_one,
* possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
*
* \param flowcontrol Type of flowcontrol used, default is
* flowcontrol_none, possible values are: flowcontrol_none,
* flowcontrol_software, flowcontrol_hardware
*
* \throw serial::PortNotOpenedException
* \throw serial::IOException
* \throw std::invalid_argument
*/
Serial (const std::string &port = "",
uint32_t baudrate = 9600,
Timeout timeout = Timeout(),
bytesize_t bytesize = eightbits,
parity_t parity = parity_none,
stopbits_t stopbits = stopbits_one,
flowcontrol_t flowcontrol = flowcontrol_none);
/*! Destructor */
virtual ~Serial ();
/*!
* Opens the serial port as long as the port is set and the port isn't
* already open.
*
* If the port is provided to the constructor then an explicit call to open
* is not needed.
*
* \see Serial::Serial
*
* \throw std::invalid_argument
* \throw serial::SerialException
* \throw serial::IOException
*/
void
open ();
/*! Gets the open status of the serial port.
*
* \return Returns true if the port is open, false otherwise.
*/
bool
isOpen () const;
/*! Closes the serial port. */
void
close ();
/*! Return the number of characters in the buffer. */
size_t
available ();
/*! Block until there is serial data to read or read_timeout_constant
* number of milliseconds have elapsed. The return value is true when
* the function exits with the port in a readable state, false otherwise
* (due to timeout or select interruption). */
bool
waitReadable ();
/*! Block for a period of time corresponding to the transmission time of
* count characters at present serial settings. This may be used in con-
* junction with waitReadable to read larger blocks of data from the
* port. */
void
waitByteTimes (size_t count);
/*! Read a given amount of bytes from the serial port into a given buffer.
*
* The read function will return in one of three cases:
* * The number of requested bytes was read.
* * In this case the number of bytes requested will match the size_t
* returned by read.
* * A timeout occurred, in this case the number of bytes read will not
* match the amount requested, but no exception will be thrown. One of
* two possible timeouts occurred:
* * The inter byte timeout expired, this means that number of
* milliseconds elapsed between receiving bytes from the serial port
* exceeded the inter byte timeout.
* * The total timeout expired, which is calculated by multiplying the
* read timeout multiplier by the number of requested bytes and then
* added to the read timeout constant. If that total number of
* milliseconds elapses after the initial call to read a timeout will
* occur.
* * An exception occurred, in this case an actual exception will be thrown.
*
* \param buffer An uint8_t array of at least the requested size.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
size_t
read (uint8_t *buffer, size_t size);
/*! Read a given amount of bytes from the serial port into a give buffer.
*
* \param buffer A reference to a std::vector of uint8_t.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
size_t
read (std::vector<uint8_t> &buffer, size_t size = 1);
/*! Read a given amount of bytes from the serial port into a give buffer.
*
* \param buffer A reference to a std::string.
* \param size A size_t defining how many bytes to be read.
*
* \return A size_t representing the number of bytes read as a result of the
* call to read.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
size_t
read (std::string &buffer, size_t size = 1);
/*! Read a given amount of bytes from the serial port and return a string
* containing the data.
*
* \param size A size_t defining how many bytes to be read.
*
* \return A std::string containing the data read from the port.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
std::string
read (size_t size = 1);
/*! Reads in a line or until a given delimiter has been processed.
*
* Reads from the serial port until a single line has been read.
*
* \param buffer A std::string reference used to store the data.
* \param size A maximum length of a line, defaults to 65536 (2^16)
* \param eol A string to match against for the EOL.
*
* \return A size_t representing the number of bytes read.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
size_t
readline (std::string &buffer, size_t size = 65536, std::string eol = "\n");
/*! Reads in a line or until a given delimiter has been processed.
*
* Reads from the serial port until a single line has been read.
*
* \param size A maximum length of a line, defaults to 65536 (2^16)
* \param eol A string to match against for the EOL.
*
* \return A std::string containing the line.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
std::string
readline (size_t size = 65536, std::string eol = "\n");
/*! Reads in multiple lines until the serial port times out.
*
* This requires a timeout > 0 before it can be run. It will read until a
* timeout occurs and return a list of strings.
*
* \param size A maximum length of combined lines, defaults to 65536 (2^16)
*
* \param eol A string to match against for the EOL.
*
* \return A vector<string> containing the lines.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
*/
std::vector<std::string>
readlines (size_t size = 65536, std::string eol = "\n");
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \param size A size_t that indicates how many bytes should be written from
* the given data buffer.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
* \throw serial::IOException
*/
size_t
write (const uint8_t *data, size_t size);
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
* \throw serial::IOException
*/
size_t
write (const std::vector<uint8_t> &data);
/*! Write a string to the serial port.
*
* \param data A const reference containing the data to be written
* to the serial port.
*
* \return A size_t representing the number of bytes actually written to
* the serial port.
*
* \throw serial::PortNotOpenedException
* \throw serial::SerialException
* \throw serial::IOException
*/
size_t
write (const std::string &data);
/*! Sets the serial port identifier.
*
* \param port A const std::string reference containing the address of the
* serial port, which would be something like 'COM1' on Windows and
* '/dev/ttyS0' on Linux.
*
* \throw std::invalid_argument
*/
void
setPort (const std::string &port);
/*! Gets the serial port identifier.
*
* \see Serial::setPort
*
* \throw std::invalid_argument
*/
std::string
getPort () const;
/*! Sets the timeout for reads and writes using the Timeout struct.
*
* There are two timeout conditions described here:
* * The inter byte timeout:
* * The inter_byte_timeout component of serial::Timeout defines the
* maximum amount of time, in milliseconds, between receiving bytes on
* the serial port that can pass before a timeout occurs. Setting this
* to zero will prevent inter byte timeouts from occurring.
* * Total time timeout:
* * The constant and multiplier component of this timeout condition,
* for both read and write, are defined in serial::Timeout. This
* timeout occurs if the total time since the read or write call was
* made exceeds the specified time in milliseconds.
* * The limit is defined by multiplying the multiplier component by the
* number of requested bytes and adding that product to the constant
* component. In this way if you want a read call, for example, to
* timeout after exactly one second regardless of the number of bytes
* you asked for then set the read_timeout_constant component of
* serial::Timeout to 1000 and the read_timeout_multiplier to zero.
* This timeout condition can be used in conjunction with the inter
* byte timeout condition with out any problems, timeout will simply
* occur when one of the two timeout conditions is met. This allows
* users to have maximum control over the trade-off between
* responsiveness and efficiency.
*
* Read and write functions will return in one of three cases. When the
* reading or writing is complete, when a timeout occurs, or when an
* exception occurs.
*
* A timeout of 0 enables non-blocking mode.
*
* \param timeout A serial::Timeout struct containing the inter byte
* timeout, and the read and write timeout constants and multipliers.
*
* \see serial::Timeout
*/
void
setTimeout (Timeout &timeout);
/*! Sets the timeout for reads and writes. */
void
setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant,
uint32_t read_timeout_multiplier, uint32_t write_timeout_constant,
uint32_t write_timeout_multiplier)
{
Timeout timeout(inter_byte_timeout, read_timeout_constant,
read_timeout_multiplier, write_timeout_constant,
write_timeout_multiplier);
return setTimeout(timeout);
}
/*! Gets the timeout for reads in seconds.
*
* \return A Timeout struct containing the inter_byte_timeout, and read
* and write timeout constants and multipliers.
*
* \see Serial::setTimeout
*/
Timeout
getTimeout () const;
/*! Sets the baudrate for the serial port.
*
* Possible baudrates depends on the system but some safe baudrates include:
* 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000,
* 57600, 115200
* Some other baudrates that are supported by some comports:
* 128000, 153600, 230400, 256000, 460800, 921600
*
* \param baudrate An integer that sets the baud rate for the serial port.
*
* \throw std::invalid_argument
*/
void
setBaudrate (uint32_t baudrate);
/*! Gets the baudrate for the serial port.
*
* \return An integer that sets the baud rate for the serial port.
*
* \see Serial::setBaudrate
*
* \throw std::invalid_argument
*/
uint32_t
getBaudrate () const;
/*! Sets the bytesize for the serial port.
*
* \param bytesize Size of each byte in the serial transmission of data,
* default is eightbits, possible values are: fivebits, sixbits, sevenbits,
* eightbits
*
* \throw std::invalid_argument
*/
void
setBytesize (bytesize_t bytesize);
/*! Gets the bytesize for the serial port.
*
* \see Serial::setBytesize
*
* \throw std::invalid_argument
*/
bytesize_t
getBytesize () const;
/*! Sets the parity for the serial port.
*
* \param parity Method of parity, default is parity_none, possible values
* are: parity_none, parity_odd, parity_even
*
* \throw std::invalid_argument
*/
void
setParity (parity_t parity);
/*! Gets the parity for the serial port.
*
* \see Serial::setParity
*
* \throw std::invalid_argument
*/
parity_t
getParity () const;
/*! Sets the stopbits for the serial port.
*
* \param stopbits Number of stop bits used, default is stopbits_one,
* possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
*
* \throw std::invalid_argument
*/
void
setStopbits (stopbits_t stopbits);
/*! Gets the stopbits for the serial port.
*
* \see Serial::setStopbits
*
* \throw std::invalid_argument
*/
stopbits_t
getStopbits () const;
/*! Sets the flow control for the serial port.
*
* \param flowcontrol Type of flowcontrol used, default is flowcontrol_none,
* possible values are: flowcontrol_none, flowcontrol_software,
* flowcontrol_hardware
*
* \throw std::invalid_argument
*/
void
setFlowcontrol (flowcontrol_t flowcontrol);
/*! Gets the flow control for the serial port.
*
* \see Serial::setFlowcontrol
*
* \throw std::invalid_argument
*/
flowcontrol_t
getFlowcontrol () const;
/*! Flush the input and output buffers */
void
flush ();
/*! Flush only the input buffer */
void
flushInput ();
/*! Flush only the output buffer */
void
flushOutput ();
/*! Sends the RS-232 break signal. See tcsendbreak(3). */
void
sendBreak (int duration);
/*! Set the break condition to a given level. Defaults to true. */
void
setBreak (bool level = true);
/*! Set the RTS handshaking line to the given level. Defaults to true. */
void
setRTS (bool level = true);
/*! Set the DTR handshaking line to the given level. Defaults to true. */
void
setDTR (bool level = true);
/*!
* Blocks until CTS, DSR, RI, CD changes or something interrupts it.
*
* Can throw an exception if an error occurs while waiting.
* You can check the status of CTS, DSR, RI, and CD once this returns.
* Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a
* resolution of less than +-1ms and as good as +-0.2ms. Otherwise a
* polling method is used which can give +-2ms.
*
* \return Returns true if one of the lines changed, false if something else
* occurred.
*
* \throw SerialException
*/
bool
waitForChange ();
/*! Returns the current status of the CTS line. */
bool
getCTS ();
/*! Returns the current status of the DSR line. */
bool
getDSR ();
/*! Returns the current status of the RI line. */
bool
getRI ();
/*! Returns the current status of the CD line. */
bool
getCD ();
private:
// Disable copy constructors
Serial(const Serial&);
Serial& operator=(const Serial&);
// Pimpl idiom, d_pointer
class SerialImpl;
SerialImpl *pimpl_;
// Scoped Lock Classes
class ScopedReadLock;
class ScopedWriteLock;
// Read common function
size_t
read_ (uint8_t *buffer, size_t size);
// Write common function
size_t
write_ (const uint8_t *data, size_t length);
};
class SerialException : public std::exception
{
// Disable copy constructors
SerialException& operator=(const SerialException&);
std::string e_what_;
public:
SerialException (const char *description) {
std::stringstream ss;
ss << "SerialException " << description << " failed.";
e_what_ = ss.str();
}
SerialException (const SerialException& other) : e_what_(other.e_what_) {}
virtual ~SerialException() throw() {}
virtual const char* what () const throw () {
return e_what_.c_str();
}
};
class IOException : public std::exception
{
// Disable copy constructors
IOException& operator=(const IOException&);
std::string file_;
int line_;
std::string e_what_;
int errno_;
public:
explicit IOException (std::string file, int line, int errnum)
: file_(file), line_(line), errno_(errnum) {
std::stringstream ss;
#if defined(_WIN32) && !defined(__MINGW32__)
char error_str [1024];
strerror_s(error_str, 1024, errnum);
#else
char * error_str = strerror(errnum);
#endif
ss << "IO Exception (" << errno_ << "): " << error_str;
ss << ", file " << file_ << ", line " << line_ << ".";
e_what_ = ss.str();
}
explicit IOException (std::string file, int line, const char * description)
: file_(file), line_(line), errno_(0) {
std::stringstream ss;
ss << "IO Exception: " << description;
ss << ", file " << file_ << ", line " << line_ << ".";
e_what_ = ss.str();
}
virtual ~IOException() throw() {}
IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}
int getErrorNumber () const { return errno_; }
virtual const char* what () const throw () {
return e_what_.c_str();
}
};
class PortNotOpenedException : public std::exception
{
// Disable copy constructors
const PortNotOpenedException& operator=(PortNotOpenedException);
std::string e_what_;
public:
PortNotOpenedException (const char * description) {
std::stringstream ss;
ss << "PortNotOpenedException " << description << " failed.";
e_what_ = ss.str();
}
PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {}
virtual ~PortNotOpenedException() throw() {}
virtual const char* what () const throw () {
return e_what_.c_str();
}
};
/*!
* Structure that describes a serial device.
*/
struct PortInfo {
/*! Address of the serial port (this can be passed to the constructor of Serial). */
std::string port;
/*! Human readable description of serial device if available. */
std::string description;
/*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */
std::string hardware_id;
};
/* Lists the serial ports available on the system
*
* Returns a vector of available serial ports, each represented
* by a serial::PortInfo data structure:
*
* \return vector of serial::PortInfo.
*/
std::vector<PortInfo>
list_ports();
} // namespace serial
#endif

View File

@@ -0,0 +1,57 @@
// This header is from the v8 google project:
// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h
// Copyright 2012 the V8 project authors. All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of Google Inc. nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// Load definitions of standard types.
#ifndef V8STDINT_H_
#define V8STDINT_H_
#include <stddef.h>
#include <stdio.h>
#if defined(_WIN32) && !defined(__MINGW32__)
typedef signed char int8_t;
typedef unsigned char uint8_t;
typedef short int16_t; // NOLINT
typedef unsigned short uint16_t; // NOLINT
typedef int int32_t;
typedef unsigned int uint32_t;
typedef __int64 int64_t;
typedef unsigned __int64 uint64_t;
// intptr_t and friends are defined in crtdefs.h through stdio.h.
#else
#include <stdint.h>
#endif
#endif // V8STDINT_H_

View File

@@ -0,0 +1,31 @@
project "serial"
kind "StaticLib"
includedirs {"include"}
if os.is("Windows") then
files{
"src/impl/win.cc",
"src/impl/list_ports/list_ports_win.cc"
}
end
if os.is("Linux") then
files{
"src/impl/unix.cc",
"src/impl/list_ports/list_ports_linux.cc"
}
end
if os.is("MacOSX") then
files{
"src/impl/unix.cc",
"src/impl/list_ports/list_ports_osx.cc"
}
end
files {
"src/serial.cc",
"**.h"
}

View File

@@ -0,0 +1,335 @@
#if defined(__linux__)
/*
* Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
* This software is made available under the terms of the MIT licence.
* A copy of the licence can be obtained from:
* http://opensource.org/licenses/MIT
*/
#include <vector>
#include <string>
#include <sstream>
#include <stdexcept>
#include <iostream>
#include <fstream>
#include <cstdio>
#include <cstdarg>
#include <cstdlib>
#include <glob.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include "serial/serial.h"
using serial::PortInfo;
using std::istringstream;
using std::ifstream;
using std::getline;
using std::vector;
using std::string;
using std::cout;
using std::endl;
static vector<string> glob(const vector<string>& patterns);
static string basename(const string& path);
static string dirname(const string& path);
static bool path_exists(const string& path);
static string realpath(const string& path);
static string usb_sysfs_friendly_name(const string& sys_usb_path);
static vector<string> get_sysfs_info(const string& device_path);
static string read_line(const string& file);
static string usb_sysfs_hw_string(const string& sysfs_path);
static string format(const char* format, ...);
vector<string>
glob(const vector<string>& patterns)
{
vector<string> paths_found;
if(patterns.size() == 0)
return paths_found;
glob_t glob_results;
int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results);
vector<string>::const_iterator iter = patterns.begin();
while(++iter != patterns.end())
{
glob_retval = glob(iter->c_str(), GLOB_APPEND, NULL, &glob_results);
}
for(int path_index = 0; path_index < glob_results.gl_pathc; path_index++)
{
paths_found.push_back(glob_results.gl_pathv[path_index]);
}
globfree(&glob_results);
return paths_found;
}
string
basename(const string& path)
{
size_t pos = path.rfind("/");
if(pos == std::string::npos)
return path;
return string(path, pos+1, string::npos);
}
string
dirname(const string& path)
{
size_t pos = path.rfind("/");
if(pos == std::string::npos)
return path;
else if(pos == 0)
return "/";
return string(path, 0, pos);
}
bool
path_exists(const string& path)
{
struct stat sb;
if( stat(path.c_str(), &sb ) == 0 )
return true;
return false;
}
string
realpath(const string& path)
{
char* real_path = realpath(path.c_str(), NULL);
string result;
if(real_path != NULL)
{
result = real_path;
free(real_path);
}
return result;
}
string
usb_sysfs_friendly_name(const string& sys_usb_path)
{
unsigned int device_number = 0;
istringstream( read_line(sys_usb_path + "/devnum") ) >> device_number;
string manufacturer = read_line( sys_usb_path + "/manufacturer" );
string product = read_line( sys_usb_path + "/product" );
string serial = read_line( sys_usb_path + "/serial" );
if( manufacturer.empty() && product.empty() && serial.empty() )
return "";
return format("%s %s %s", manufacturer.c_str(), product.c_str(), serial.c_str() );
}
vector<string>
get_sysfs_info(const string& device_path)
{
string device_name = basename( device_path );
string friendly_name;
string hardware_id;
string sys_device_path = format( "/sys/class/tty/%s/device", device_name.c_str() );
if( device_name.compare(0,6,"ttyUSB") == 0 )
{
sys_device_path = dirname( dirname( realpath( sys_device_path ) ) );
if( path_exists( sys_device_path ) )
{
friendly_name = usb_sysfs_friendly_name( sys_device_path );
hardware_id = usb_sysfs_hw_string( sys_device_path );
}
}
else if( device_name.compare(0,6,"ttyACM") == 0 )
{
sys_device_path = dirname( realpath( sys_device_path ) );
if( path_exists( sys_device_path ) )
{
friendly_name = usb_sysfs_friendly_name( sys_device_path );
hardware_id = usb_sysfs_hw_string( sys_device_path );
}
}
else
{
// Try to read ID string of PCI device
string sys_id_path = sys_device_path + "/id";
if( path_exists( sys_id_path ) )
hardware_id = read_line( sys_id_path );
}
if( friendly_name.empty() )
friendly_name = device_name;
if( hardware_id.empty() )
hardware_id = "n/a";
vector<string> result;
result.push_back(friendly_name);
result.push_back(hardware_id);
return result;
}
string
read_line(const string& file)
{
ifstream ifs(file.c_str(), ifstream::in);
string line;
if(ifs)
{
getline(ifs, line);
}
return line;
}
string
format(const char* format, ...)
{
va_list ap;
size_t buffer_size_bytes = 256;
string result;
char* buffer = (char*)malloc(buffer_size_bytes);
if( buffer == NULL )
return result;
bool done = false;
unsigned int loop_count = 0;
while(!done)
{
va_start(ap, format);
int return_value = vsnprintf(buffer, buffer_size_bytes, format, ap);
if( return_value < 0 )
{
done = true;
}
else if( return_value >= buffer_size_bytes )
{
// Realloc and try again.
buffer_size_bytes = return_value + 1;
char* new_buffer_ptr = (char*)realloc(buffer, buffer_size_bytes);
if( new_buffer_ptr == NULL )
{
done = true;
}
else
{
buffer = new_buffer_ptr;
}
}
else
{
result = buffer;
done = true;
}
va_end(ap);
if( ++loop_count > 5 )
done = true;
}
free(buffer);
return result;
}
string
usb_sysfs_hw_string(const string& sysfs_path)
{
string serial_number = read_line( sysfs_path + "/serial" );
if( serial_number.length() > 0 )
{
serial_number = format( "SNR=%s", serial_number.c_str() );
}
string vid = read_line( sysfs_path + "/idVendor" );
string pid = read_line( sysfs_path + "/idProduct" );
return format("USB VID:PID=%s:%s %s", vid.c_str(), pid.c_str(), serial_number.c_str() );
}
vector<PortInfo>
serial::list_ports()
{
vector<PortInfo> results;
vector<string> search_globs;
search_globs.push_back("/dev/ttyACM*");
search_globs.push_back("/dev/ttyS*");
search_globs.push_back("/dev/ttyUSB*");
search_globs.push_back("/dev/tty.*");
search_globs.push_back("/dev/cu.*");
vector<string> devices_found = glob( search_globs );
vector<string>::iterator iter = devices_found.begin();
while( iter != devices_found.end() )
{
string device = *iter++;
vector<string> sysfs_info = get_sysfs_info( device );
string friendly_name = sysfs_info[0];
string hardware_id = sysfs_info[1];
PortInfo device_entry;
device_entry.port = device;
device_entry.description = friendly_name;
device_entry.hardware_id = hardware_id;
results.push_back( device_entry );
}
return results;
}
#endif // defined(__linux__)

View File

@@ -0,0 +1,286 @@
#if defined(__APPLE__)
#include <sys/param.h>
#include <stdint.h>
#include <CoreFoundation/CoreFoundation.h>
#include <IOKit/IOKitLib.h>
#include <IOKit/serial/IOSerialKeys.h>
#include <IOKit/IOBSD.h>
#include <iostream>
#include <string>
#include <vector>
#include "serial/serial.h"
using serial::PortInfo;
using std::string;
using std::vector;
#define HARDWARE_ID_STRING_LENGTH 128
string cfstring_to_string( CFStringRef cfstring );
string get_device_path( io_object_t& serial_port );
string get_class_name( io_object_t& obj );
io_registry_entry_t get_parent_iousb_device( io_object_t& serial_port );
string get_string_property( io_object_t& device, const char* property );
uint16_t get_int_property( io_object_t& device, const char* property );
string rtrim(const string& str);
string
cfstring_to_string( CFStringRef cfstring )
{
char cstring[MAXPATHLEN];
string result;
if( cfstring )
{
Boolean success = CFStringGetCString( cfstring,
cstring,
sizeof(cstring),
kCFStringEncodingASCII );
if( success )
result = cstring;
}
return result;
}
string
get_device_path( io_object_t& serial_port )
{
CFTypeRef callout_path;
string device_path;
callout_path = IORegistryEntryCreateCFProperty( serial_port,
CFSTR(kIOCalloutDeviceKey),
kCFAllocatorDefault,
0 );
if (callout_path)
{
if( CFGetTypeID(callout_path) == CFStringGetTypeID() )
device_path = cfstring_to_string( static_cast<CFStringRef>(callout_path) );
CFRelease(callout_path);
}
return device_path;
}
string
get_class_name( io_object_t& obj )
{
string result;
io_name_t class_name;
kern_return_t kern_result;
kern_result = IOObjectGetClass( obj, class_name );
if( kern_result == KERN_SUCCESS )
result = class_name;
return result;
}
io_registry_entry_t
get_parent_iousb_device( io_object_t& serial_port )
{
io_object_t device = serial_port;
io_registry_entry_t parent = 0;
io_registry_entry_t result = 0;
kern_return_t kern_result = KERN_FAILURE;
string name = get_class_name(device);
// Walk the IO Registry tree looking for this devices parent IOUSBDevice.
while( name != "IOUSBDevice" )
{
kern_result = IORegistryEntryGetParentEntry( device,
kIOServicePlane,
&parent );
if(kern_result != KERN_SUCCESS)
{
result = 0;
break;
}
device = parent;
name = get_class_name(device);
}
if(kern_result == KERN_SUCCESS)
result = device;
return result;
}
string
get_string_property( io_object_t& device, const char* property )
{
string property_name;
if( device )
{
CFStringRef property_as_cfstring = CFStringCreateWithCString (
kCFAllocatorDefault,
property,
kCFStringEncodingASCII );
CFTypeRef name_as_cfstring = IORegistryEntryCreateCFProperty(
device,
property_as_cfstring,
kCFAllocatorDefault,
0 );
if( name_as_cfstring )
{
if( CFGetTypeID(name_as_cfstring) == CFStringGetTypeID() )
property_name = cfstring_to_string( static_cast<CFStringRef>(name_as_cfstring) );
CFRelease(name_as_cfstring);
}
if(property_as_cfstring)
CFRelease(property_as_cfstring);
}
return property_name;
}
uint16_t
get_int_property( io_object_t& device, const char* property )
{
uint16_t result = 0;
if( device )
{
CFStringRef property_as_cfstring = CFStringCreateWithCString (
kCFAllocatorDefault,
property,
kCFStringEncodingASCII );
CFTypeRef number = IORegistryEntryCreateCFProperty( device,
property_as_cfstring,
kCFAllocatorDefault,
0 );
if(property_as_cfstring)
CFRelease(property_as_cfstring);
if( number )
{
if( CFGetTypeID(number) == CFNumberGetTypeID() )
{
bool success = CFNumberGetValue( static_cast<CFNumberRef>(number),
kCFNumberSInt16Type,
&result );
if( !success )
result = 0;
}
CFRelease(number);
}
}
return result;
}
string rtrim(const string& str)
{
string result = str;
string whitespace = " \t\f\v\n\r";
std::size_t found = result.find_last_not_of(whitespace);
if (found != std::string::npos)
result.erase(found+1);
else
result.clear();
return result;
}
vector<PortInfo>
serial::list_ports(void)
{
vector<PortInfo> devices_found;
CFMutableDictionaryRef classes_to_match;
io_iterator_t serial_port_iterator;
io_object_t serial_port;
mach_port_t master_port;
kern_return_t kern_result;
kern_result = IOMasterPort(MACH_PORT_NULL, &master_port);
if(kern_result != KERN_SUCCESS)
return devices_found;
classes_to_match = IOServiceMatching(kIOSerialBSDServiceValue);
if (classes_to_match == NULL)
return devices_found;
CFDictionarySetValue( classes_to_match,
CFSTR(kIOSerialBSDTypeKey),
CFSTR(kIOSerialBSDAllTypes) );
kern_result = IOServiceGetMatchingServices(master_port, classes_to_match, &serial_port_iterator);
if (KERN_SUCCESS != kern_result)
return devices_found;
while ( (serial_port = IOIteratorNext(serial_port_iterator)) )
{
string device_path = get_device_path( serial_port );
io_registry_entry_t parent = get_parent_iousb_device( serial_port );
IOObjectRelease(serial_port);
if( device_path.empty() )
continue;
PortInfo port_info;
port_info.port = device_path;
port_info.description = "n/a";
port_info.hardware_id = "n/a";
string device_name = rtrim( get_string_property( parent, "USB Product Name" ) );
string vendor_name = rtrim( get_string_property( parent, "USB Vendor Name") );
string description = rtrim( vendor_name + " " + device_name );
if( !description.empty() )
port_info.description = description;
string serial_number = rtrim(get_string_property( parent, "USB Serial Number" ) );
uint16_t vendor_id = get_int_property( parent, "idVendor" );
uint16_t product_id = get_int_property( parent, "idProduct" );
if( vendor_id && product_id )
{
char cstring[HARDWARE_ID_STRING_LENGTH];
if(serial_number.empty())
serial_number = "None";
int ret = snprintf( cstring, HARDWARE_ID_STRING_LENGTH, "USB VID:PID=%04x:%04x SNR=%s",
vendor_id,
product_id,
serial_number.c_str() );
if( (ret >= 0) && (ret < HARDWARE_ID_STRING_LENGTH) )
port_info.hardware_id = cstring;
}
devices_found.push_back(port_info);
}
IOObjectRelease(serial_port_iterator);
return devices_found;
}
#endif // defined(__APPLE__)

View File

@@ -0,0 +1,152 @@
#if defined(_WIN32)
/*
* Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
* This software is made available under the terms of the MIT licence.
* A copy of the licence can be obtained from:
* http://opensource.org/licenses/MIT
*/
#include "serial/serial.h"
#include <tchar.h>
#include <windows.h>
#include <setupapi.h>
#include <initguid.h>
#include <devguid.h>
#include <cstring>
using serial::PortInfo;
using std::vector;
using std::string;
static const DWORD port_name_max_length = 256;
static const DWORD friendly_name_max_length = 256;
static const DWORD hardware_id_max_length = 256;
// Convert a wide Unicode string to an UTF8 string
std::string utf8_encode(const std::wstring &wstr)
{
int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL);
std::string strTo( size_needed, 0 );
WideCharToMultiByte (CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL);
return strTo;
}
vector<PortInfo>
serial::list_ports()
{
vector<PortInfo> devices_found;
HDEVINFO device_info_set = SetupDiGetClassDevs(
(const GUID *) &GUID_DEVCLASS_PORTS,
NULL,
NULL,
DIGCF_PRESENT);
unsigned int device_info_set_index = 0;
SP_DEVINFO_DATA device_info_data;
device_info_data.cbSize = sizeof(SP_DEVINFO_DATA);
while(SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data))
{
device_info_set_index++;
// Get port name
HKEY hkey = SetupDiOpenDevRegKey(
device_info_set,
&device_info_data,
DICS_FLAG_GLOBAL,
0,
DIREG_DEV,
KEY_READ);
TCHAR port_name[port_name_max_length];
DWORD port_name_length = port_name_max_length;
LONG return_code = RegQueryValueEx(
hkey,
_T("PortName"),
NULL,
NULL,
(LPBYTE)port_name,
&port_name_length);
RegCloseKey(hkey);
if(return_code != EXIT_SUCCESS)
continue;
if(port_name_length > 0 && port_name_length <= port_name_max_length)
port_name[port_name_length-1] = '\0';
else
port_name[0] = '\0';
// Ignore parallel ports
if(_tcsstr(port_name, _T("LPT")) != NULL)
continue;
// Get port friendly name
TCHAR friendly_name[friendly_name_max_length];
DWORD friendly_name_actual_length = 0;
BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty(
device_info_set,
&device_info_data,
SPDRP_FRIENDLYNAME,
NULL,
(PBYTE)friendly_name,
friendly_name_max_length,
&friendly_name_actual_length);
if(got_friendly_name == TRUE && friendly_name_actual_length > 0)
friendly_name[friendly_name_actual_length-1] = '\0';
else
friendly_name[0] = '\0';
// Get hardware ID
TCHAR hardware_id[hardware_id_max_length];
DWORD hardware_id_actual_length = 0;
BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty(
device_info_set,
&device_info_data,
SPDRP_HARDWAREID,
NULL,
(PBYTE)hardware_id,
hardware_id_max_length,
&hardware_id_actual_length);
if(got_hardware_id == TRUE && hardware_id_actual_length > 0)
hardware_id[hardware_id_actual_length-1] = '\0';
else
hardware_id[0] = '\0';
#ifdef UNICODE
std::string portName = utf8_encode(port_name);
std::string friendlyName = utf8_encode(friendly_name);
std::string hardwareId = utf8_encode(hardware_id);
#else
std::string portName = port_name;
std::string friendlyName = friendly_name;
std::string hardwareId = hardware_id;
#endif
PortInfo port_entry;
port_entry.port = portName;
port_entry.description = friendlyName;
port_entry.hardware_id = hardwareId;
devices_found.push_back(port_entry);
}
SetupDiDestroyDeviceInfoList(device_info_set);
return devices_found;
}
#endif // #if defined(_WIN32)

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,640 @@
#if defined(_WIN32)
/* Copyright 2012 William Woodall and John Harrison */
#include <sstream>
#include "serial/impl/win.h"
using std::string;
using std::wstring;
using std::stringstream;
using std::invalid_argument;
using serial::Serial;
using serial::Timeout;
using serial::bytesize_t;
using serial::parity_t;
using serial::stopbits_t;
using serial::flowcontrol_t;
using serial::SerialException;
using serial::PortNotOpenedException;
using serial::IOException;
inline wstring
_prefix_port_if_needed(const wstring &input)
{
static wstring windows_com_port_prefix = L"\\\\.\\";
if (input.compare(windows_com_port_prefix) != 0)
{
return windows_com_port_prefix + input;
}
return input;
}
Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate,
bytesize_t bytesize,
parity_t parity, stopbits_t stopbits,
flowcontrol_t flowcontrol)
: port_ (port.begin(), port.end()), fd_ (INVALID_HANDLE_VALUE), is_open_ (false),
baudrate_ (baudrate), parity_ (parity),
bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol)
{
if (port_.empty () == false)
open ();
read_mutex = CreateMutex(NULL, false, NULL);
write_mutex = CreateMutex(NULL, false, NULL);
}
Serial::SerialImpl::~SerialImpl ()
{
this->close();
CloseHandle(read_mutex);
CloseHandle(write_mutex);
}
void
Serial::SerialImpl::open ()
{
if (port_.empty ()) {
throw invalid_argument ("Empty port is invalid.");
}
if (is_open_ == true) {
throw SerialException ("Serial port already open.");
}
// See: https://github.com/wjwwood/serial/issues/84
wstring port_with_prefix = _prefix_port_if_needed(port_);
LPCWSTR lp_port = port_with_prefix.c_str();
fd_ = CreateFileW(lp_port,
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
0);
if (fd_ == INVALID_HANDLE_VALUE) {
DWORD errno_ = GetLastError();
stringstream ss;
switch (errno_) {
case ERROR_FILE_NOT_FOUND:
// Use this->getPort to convert to a std::string
ss << "Specified port, " << this->getPort() << ", does not exist.";
THROW (IOException, ss.str().c_str());
default:
ss << "Unknown error opening the serial port: " << errno;
THROW (IOException, ss.str().c_str());
}
}
reconfigurePort();
is_open_ = true;
}
void
Serial::SerialImpl::reconfigurePort ()
{
if (fd_ == INVALID_HANDLE_VALUE) {
// Can only operate on a valid file descriptor
THROW (IOException, "Invalid file descriptor, is the serial port open?");
}
DCB dcbSerialParams = {0};
dcbSerialParams.DCBlength=sizeof(dcbSerialParams);
if (!GetCommState(fd_, &dcbSerialParams)) {
//error getting state
THROW (IOException, "Error getting the serial port state.");
}
// setup baud rate
switch (baudrate_) {
#ifdef CBR_0
case 0: dcbSerialParams.BaudRate = CBR_0; break;
#endif
#ifdef CBR_50
case 50: dcbSerialParams.BaudRate = CBR_50; break;
#endif
#ifdef CBR_75
case 75: dcbSerialParams.BaudRate = CBR_75; break;
#endif
#ifdef CBR_110
case 110: dcbSerialParams.BaudRate = CBR_110; break;
#endif
#ifdef CBR_134
case 134: dcbSerialParams.BaudRate = CBR_134; break;
#endif
#ifdef CBR_150
case 150: dcbSerialParams.BaudRate = CBR_150; break;
#endif
#ifdef CBR_200
case 200: dcbSerialParams.BaudRate = CBR_200; break;
#endif
#ifdef CBR_300
case 300: dcbSerialParams.BaudRate = CBR_300; break;
#endif
#ifdef CBR_600
case 600: dcbSerialParams.BaudRate = CBR_600; break;
#endif
#ifdef CBR_1200
case 1200: dcbSerialParams.BaudRate = CBR_1200; break;
#endif
#ifdef CBR_1800
case 1800: dcbSerialParams.BaudRate = CBR_1800; break;
#endif
#ifdef CBR_2400
case 2400: dcbSerialParams.BaudRate = CBR_2400; break;
#endif
#ifdef CBR_4800
case 4800: dcbSerialParams.BaudRate = CBR_4800; break;
#endif
#ifdef CBR_7200
case 7200: dcbSerialParams.BaudRate = CBR_7200; break;
#endif
#ifdef CBR_9600
case 9600: dcbSerialParams.BaudRate = CBR_9600; break;
#endif
#ifdef CBR_14400
case 14400: dcbSerialParams.BaudRate = CBR_14400; break;
#endif
#ifdef CBR_19200
case 19200: dcbSerialParams.BaudRate = CBR_19200; break;
#endif
#ifdef CBR_28800
case 28800: dcbSerialParams.BaudRate = CBR_28800; break;
#endif
#ifdef CBR_57600
case 57600: dcbSerialParams.BaudRate = CBR_57600; break;
#endif
#ifdef CBR_76800
case 76800: dcbSerialParams.BaudRate = CBR_76800; break;
#endif
#ifdef CBR_38400
case 38400: dcbSerialParams.BaudRate = CBR_38400; break;
#endif
#ifdef CBR_115200
case 115200: dcbSerialParams.BaudRate = CBR_115200; break;
#endif
#ifdef CBR_128000
case 128000: dcbSerialParams.BaudRate = CBR_128000; break;
#endif
#ifdef CBR_153600
case 153600: dcbSerialParams.BaudRate = CBR_153600; break;
#endif
#ifdef CBR_230400
case 230400: dcbSerialParams.BaudRate = CBR_230400; break;
#endif
#ifdef CBR_256000
case 256000: dcbSerialParams.BaudRate = CBR_256000; break;
#endif
#ifdef CBR_460800
case 460800: dcbSerialParams.BaudRate = CBR_460800; break;
#endif
#ifdef CBR_921600
case 921600: dcbSerialParams.BaudRate = CBR_921600; break;
#endif
default:
// Try to blindly assign it
dcbSerialParams.BaudRate = baudrate_;
}
// setup char len
if (bytesize_ == eightbits)
dcbSerialParams.ByteSize = 8;
else if (bytesize_ == sevenbits)
dcbSerialParams.ByteSize = 7;
else if (bytesize_ == sixbits)
dcbSerialParams.ByteSize = 6;
else if (bytesize_ == fivebits)
dcbSerialParams.ByteSize = 5;
else
throw invalid_argument ("invalid char len");
// setup stopbits
if (stopbits_ == stopbits_one)
dcbSerialParams.StopBits = ONESTOPBIT;
else if (stopbits_ == stopbits_one_point_five)
dcbSerialParams.StopBits = ONE5STOPBITS;
else if (stopbits_ == stopbits_two)
dcbSerialParams.StopBits = TWOSTOPBITS;
else
throw invalid_argument ("invalid stop bit");
// setup parity
if (parity_ == parity_none) {
dcbSerialParams.Parity = NOPARITY;
} else if (parity_ == parity_even) {
dcbSerialParams.Parity = EVENPARITY;
} else if (parity_ == parity_odd) {
dcbSerialParams.Parity = ODDPARITY;
} else if (parity_ == parity_mark) {
dcbSerialParams.Parity = MARKPARITY;
} else if (parity_ == parity_space) {
dcbSerialParams.Parity = SPACEPARITY;
} else {
throw invalid_argument ("invalid parity");
}
// setup flowcontrol
if (flowcontrol_ == flowcontrol_none) {
dcbSerialParams.fOutxCtsFlow = false;
dcbSerialParams.fRtsControl = 0x00;
dcbSerialParams.fOutX = false;
dcbSerialParams.fInX = false;
}
if (flowcontrol_ == flowcontrol_software) {
dcbSerialParams.fOutxCtsFlow = false;
dcbSerialParams.fRtsControl = 0x00;
dcbSerialParams.fOutX = true;
dcbSerialParams.fInX = true;
}
if (flowcontrol_ == flowcontrol_hardware) {
dcbSerialParams.fOutxCtsFlow = true;
dcbSerialParams.fRtsControl = 0x03;
dcbSerialParams.fOutX = false;
dcbSerialParams.fInX = false;
}
// activate settings
if (!SetCommState(fd_, &dcbSerialParams)){
CloseHandle(fd_);
THROW (IOException, "Error setting serial port settings.");
}
// Setup timeouts
COMMTIMEOUTS timeouts = {0};
timeouts.ReadIntervalTimeout = timeout_.inter_byte_timeout;
timeouts.ReadTotalTimeoutConstant = timeout_.read_timeout_constant;
timeouts.ReadTotalTimeoutMultiplier = timeout_.read_timeout_multiplier;
timeouts.WriteTotalTimeoutConstant = timeout_.write_timeout_constant;
timeouts.WriteTotalTimeoutMultiplier = timeout_.write_timeout_multiplier;
if (!SetCommTimeouts(fd_, &timeouts)) {
THROW (IOException, "Error setting timeouts.");
}
}
void
Serial::SerialImpl::close ()
{
if (is_open_ == true) {
if (fd_ != INVALID_HANDLE_VALUE) {
int ret;
ret = CloseHandle(fd_);
if (ret == 0) {
stringstream ss;
ss << "Error while closing serial port: " << GetLastError();
THROW (IOException, ss.str().c_str());
} else {
fd_ = INVALID_HANDLE_VALUE;
}
}
is_open_ = false;
}
}
bool
Serial::SerialImpl::isOpen () const
{
return is_open_;
}
size_t
Serial::SerialImpl::available ()
{
if (!is_open_) {
return 0;
}
COMSTAT cs;
if (!ClearCommError(fd_, NULL, &cs)) {
stringstream ss;
ss << "Error while checking status of the serial port: " << GetLastError();
THROW (IOException, ss.str().c_str());
}
return static_cast<size_t>(cs.cbInQue);
}
bool
Serial::SerialImpl::waitReadable (uint32_t /*timeout*/)
{
THROW (IOException, "waitReadable is not implemented on Windows.");
return false;
}
void
Serial::SerialImpl::waitByteTimes (size_t /*count*/)
{
THROW (IOException, "waitByteTimes is not implemented on Windows.");
}
size_t
Serial::SerialImpl::read (uint8_t *buf, size_t size)
{
if (!is_open_) {
throw PortNotOpenedException ("Serial::read");
}
DWORD bytes_read;
if (!ReadFile(fd_, buf, static_cast<DWORD>(size), &bytes_read, NULL)) {
stringstream ss;
ss << "Error while reading from the serial port: " << GetLastError();
THROW (IOException, ss.str().c_str());
}
return (size_t) (bytes_read);
}
size_t
Serial::SerialImpl::write (const uint8_t *data, size_t length)
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::write");
}
DWORD bytes_written;
if (!WriteFile(fd_, data, static_cast<DWORD>(length), &bytes_written, NULL)) {
stringstream ss;
ss << "Error while writing to the serial port: " << GetLastError();
THROW (IOException, ss.str().c_str());
}
return (size_t) (bytes_written);
}
void
Serial::SerialImpl::setPort (const string &port)
{
port_ = wstring(port.begin(), port.end());
}
string
Serial::SerialImpl::getPort () const
{
return string(port_.begin(), port_.end());
}
void
Serial::SerialImpl::setTimeout (serial::Timeout &timeout)
{
timeout_ = timeout;
if (is_open_) {
reconfigurePort ();
}
}
serial::Timeout
Serial::SerialImpl::getTimeout () const
{
return timeout_;
}
void
Serial::SerialImpl::setBaudrate (unsigned long baudrate)
{
baudrate_ = baudrate;
if (is_open_) {
reconfigurePort ();
}
}
unsigned long
Serial::SerialImpl::getBaudrate () const
{
return baudrate_;
}
void
Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize)
{
bytesize_ = bytesize;
if (is_open_) {
reconfigurePort ();
}
}
serial::bytesize_t
Serial::SerialImpl::getBytesize () const
{
return bytesize_;
}
void
Serial::SerialImpl::setParity (serial::parity_t parity)
{
parity_ = parity;
if (is_open_) {
reconfigurePort ();
}
}
serial::parity_t
Serial::SerialImpl::getParity () const
{
return parity_;
}
void
Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits)
{
stopbits_ = stopbits;
if (is_open_) {
reconfigurePort ();
}
}
serial::stopbits_t
Serial::SerialImpl::getStopbits () const
{
return stopbits_;
}
void
Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol)
{
flowcontrol_ = flowcontrol;
if (is_open_) {
reconfigurePort ();
}
}
serial::flowcontrol_t
Serial::SerialImpl::getFlowcontrol () const
{
return flowcontrol_;
}
void
Serial::SerialImpl::flush ()
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::flush");
}
FlushFileBuffers (fd_);
}
void
Serial::SerialImpl::flushInput ()
{
THROW (IOException, "flushInput is not supported on Windows.");
}
void
Serial::SerialImpl::flushOutput ()
{
THROW (IOException, "flushOutput is not supported on Windows.");
}
void
Serial::SerialImpl::sendBreak (int /*duration*/)
{
THROW (IOException, "sendBreak is not supported on Windows.");
}
void
Serial::SerialImpl::setBreak (bool level)
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::setBreak");
}
if (level) {
EscapeCommFunction (fd_, SETBREAK);
} else {
EscapeCommFunction (fd_, CLRBREAK);
}
}
void
Serial::SerialImpl::setRTS (bool level)
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::setRTS");
}
if (level) {
EscapeCommFunction (fd_, SETRTS);
} else {
EscapeCommFunction (fd_, CLRRTS);
}
}
void
Serial::SerialImpl::setDTR (bool level)
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::setDTR");
}
if (level) {
EscapeCommFunction (fd_, SETDTR);
} else {
EscapeCommFunction (fd_, CLRDTR);
}
}
bool
Serial::SerialImpl::waitForChange ()
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::waitForChange");
}
DWORD dwCommEvent;
if (!SetCommMask(fd_, EV_CTS | EV_DSR | EV_RING | EV_RLSD)) {
// Error setting communications mask
return false;
}
if (!WaitCommEvent(fd_, &dwCommEvent, NULL)) {
// An error occurred waiting for the event.
return false;
} else {
// Event has occurred.
return true;
}
}
bool
Serial::SerialImpl::getCTS ()
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::getCTS");
}
DWORD dwModemStatus;
if (!GetCommModemStatus(fd_, &dwModemStatus)) {
THROW (IOException, "Error getting the status of the CTS line.");
}
return (MS_CTS_ON & dwModemStatus) != 0;
}
bool
Serial::SerialImpl::getDSR ()
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::getDSR");
}
DWORD dwModemStatus;
if (!GetCommModemStatus(fd_, &dwModemStatus)) {
THROW (IOException, "Error getting the status of the DSR line.");
}
return (MS_DSR_ON & dwModemStatus) != 0;
}
bool
Serial::SerialImpl::getRI()
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::getRI");
}
DWORD dwModemStatus;
if (!GetCommModemStatus(fd_, &dwModemStatus)) {
THROW (IOException, "Error getting the status of the RI line.");
}
return (MS_RING_ON & dwModemStatus) != 0;
}
bool
Serial::SerialImpl::getCD()
{
if (is_open_ == false) {
throw PortNotOpenedException ("Serial::getCD");
}
DWORD dwModemStatus;
if (!GetCommModemStatus(fd_, &dwModemStatus)) {
// Error in GetCommModemStatus;
THROW (IOException, "Error getting the status of the CD line.");
}
return (MS_RLSD_ON & dwModemStatus) != 0;
}
void
Serial::SerialImpl::readLock()
{
if (WaitForSingleObject(read_mutex, INFINITE) != WAIT_OBJECT_0) {
THROW (IOException, "Error claiming read mutex.");
}
}
void
Serial::SerialImpl::readUnlock()
{
if (!ReleaseMutex(read_mutex)) {
THROW (IOException, "Error releasing read mutex.");
}
}
void
Serial::SerialImpl::writeLock()
{
if (WaitForSingleObject(write_mutex, INFINITE) != WAIT_OBJECT_0) {
THROW (IOException, "Error claiming write mutex.");
}
}
void
Serial::SerialImpl::writeUnlock()
{
if (!ReleaseMutex(write_mutex)) {
THROW (IOException, "Error releasing write mutex.");
}
}
#endif // #if defined(_WIN32)

Some files were not shown because too many files have changed in this diff Show More