From 7578f39c9a0aa90bc4e3027f7fcce09e5294cf79 Mon Sep 17 00:00:00 2001 From: Rico P Date: Mon, 30 Oct 2017 11:09:09 +0100 Subject: [PATCH 01/30] use Ansi version of LoadLibrary I got the error "C2664: 'HMODULE LoadLibraryW(LPCWSTR)': cannot convert argument 1 from 'const char *' to 'LPCWSTR'". Mapping B3_DYNLIB_OPEN to the Ansi version of LoadLibrary fixed the error for me. --- examples/SharedMemory/b3PluginManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 7ea16c33a..afd352d41 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -13,7 +13,7 @@ typedef HMODULE B3_DYNLIB_HANDLE; - #define B3_DYNLIB_OPEN LoadLibrary + #define B3_DYNLIB_OPEN LoadLibraryA #define B3_DYNLIB_CLOSE FreeLibrary #define B3_DYNLIB_IMPORT GetProcAddress #else From 5138fe2c55548be736374978ca74538137de66c1 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 17 Nov 2017 14:45:37 +0100 Subject: [PATCH 02/30] Fix wrong depth values returned by TinyRenderer --- .../SharedMemory/TinyRendererVisualShapeConverter.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 8274e0bc3..b237e412c 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -1025,12 +1025,13 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels { if (depthBuffer) { - float distance = -m_data->m_depthBuffer[i+startPixelIndex]; float farPlane = m_data->m_camera.getCameraFrustumFar(); float nearPlane = m_data->m_camera.getCameraFrustumNear(); - - btClamp(distance,nearPlane,farPlane); - + + // TinyRenderer returns clip coordinates, transform to eye coordinates first + float z_c = -m_data->m_depthBuffer[i+startPixelIndex]; + float distance = (farPlane - nearPlane) / (farPlane + nearPlane) * (z_c + 2. * farPlane * nearPlane / (farPlane - nearPlane)); + // the depth buffer value is between 0 and 1 float a = farPlane / (farPlane - nearPlane); float b = farPlane * nearPlane / (nearPlane - farPlane); From 8257429964814b953a168d6b345b8ef2f2469f24 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 17 Nov 2017 14:49:12 +0100 Subject: [PATCH 03/30] Fix error discarding fragments in our_gl --- examples/TinyRenderer/our_gl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index a47bfe2cb..8ea08f00b 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -182,7 +182,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb bool discard = shader.fragment(bc_clip, color); if (frag_depth<-shader.m_farPlane) discard=true; - if (frag_depth>-shader.m_nearPlane) + if (frag_depth>shader.m_nearPlane) discard=true; if (!discard) { From bb2ee12b861ba56682059f3c11e30b90c9f08e80 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 17 Nov 2017 14:59:24 +0100 Subject: [PATCH 04/30] Initialize TinyRenderer's depth buffer properly. --- examples/SharedMemory/TinyRendererVisualShapeConverter.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index b237e412c..31c0ff6d0 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -772,12 +772,13 @@ void TinyRendererVisualShapeConverter::resetCamera(float camDist, float yaw, flo void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor) { + float farPlane = m_data->m_camera.getCameraFrustumFar(); for(int y=0;ym_swHeight;++y) { for(int x=0;xm_swWidth;++x) { m_data->m_rgbColorBuffer.set(x,y,clearColor); - m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f; + m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -farPlane; m_data->m_shadowBuffer[x+y*m_data->m_swWidth] = -1e30f; m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1; } @@ -806,13 +807,13 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo clearColor.bgra[2] = 255; clearColor.bgra[3] = 255; - clearBuffers(clearColor); float near = projMat[14]/(projMat[10]-1); float far = projMat[14]/(projMat[10]+1); m_data->m_camera.setCameraFrustumNear( near); m_data->m_camera.setCameraFrustumFar(far); - + + clearBuffers(clearColor); ATTRIBUTE_ALIGNED16(btScalar modelMat[16]); From f7e593f8e9f8f32e4cbb74756133c2b89c97f58d Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 17 Nov 2017 15:15:11 +0100 Subject: [PATCH 05/30] Simplify the depth value transformations to reduce the number of operations --- .../TinyRendererVisualShapeConverter.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 31c0ff6d0..d44defa34 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -1031,12 +1031,15 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels // TinyRenderer returns clip coordinates, transform to eye coordinates first float z_c = -m_data->m_depthBuffer[i+startPixelIndex]; - float distance = (farPlane - nearPlane) / (farPlane + nearPlane) * (z_c + 2. * farPlane * nearPlane / (farPlane - nearPlane)); + // float distance = (farPlane - nearPlane) / (farPlane + nearPlane) * (z_c + 2. * farPlane * nearPlane / (farPlane - nearPlane)); - // the depth buffer value is between 0 and 1 - float a = farPlane / (farPlane - nearPlane); - float b = farPlane * nearPlane / (nearPlane - farPlane); - depthBuffer[i] = a + b / distance; + // The depth buffer value is between 0 and 1 + // float a = farPlane / (farPlane - nearPlane); + // float b = farPlane * nearPlane / (nearPlane - farPlane); + // depthBuffer[i] = a + b / distance; + + // Simply the above expressions + depthBuffer[i] = farPlane * (nearPlane + z_c) / (2. * farPlane * nearPlane + farPlane * z_c - nearPlane * z_c); } if (segmentationMaskBuffer) { From e35b0c564347438414031302587c3002895747c4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 17 Nov 2017 12:09:21 -0800 Subject: [PATCH 06/30] remove obsolete 'register' from BussIK --- examples/ThirdPartyLibs/BussIK/LinearR2.cpp | 4 +- examples/ThirdPartyLibs/BussIK/LinearR2.h | 40 +++++++++---------- examples/ThirdPartyLibs/BussIK/LinearR3.cpp | 40 +++++++++---------- examples/ThirdPartyLibs/BussIK/LinearR3.h | 44 ++++++++++----------- examples/ThirdPartyLibs/BussIK/LinearR4.cpp | 8 ++-- examples/ThirdPartyLibs/BussIK/LinearR4.h | 30 +++++++------- 6 files changed, 83 insertions(+), 83 deletions(-) diff --git a/examples/ThirdPartyLibs/BussIK/LinearR2.cpp b/examples/ThirdPartyLibs/BussIK/LinearR2.cpp index 9e3398f65..0a32c7f51 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR2.cpp +++ b/examples/ThirdPartyLibs/BussIK/LinearR2.cpp @@ -51,14 +51,14 @@ LinearMapR2 LinearMapR2::Inverse() const // Returns inverse { - register double detInv = 1.0/(m11*m22 - m12*m21) ; + double detInv = 1.0/(m11*m22 - m12*m21) ; return( LinearMapR2( m22*detInv, -m21*detInv, -m12*detInv, m11*detInv ) ); } LinearMapR2& LinearMapR2::Invert() // Converts into inverse. { - register double detInv = 1.0/(m11*m22 - m12*m21) ; + double detInv = 1.0/(m11*m22 - m12*m21) ; double temp; temp = m11*detInv; diff --git a/examples/ThirdPartyLibs/BussIK/LinearR2.h b/examples/ThirdPartyLibs/BussIK/LinearR2.h index 4c4140c71..d439eebcf 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR2.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR2.h @@ -90,7 +90,7 @@ public: VectorR2& operator*= ( double m ) { x*=m; y*=m; return(*this); } VectorR2& operator/= ( double m ) - { register double mInv = 1.0/m; + { double mInv = 1.0/m; x*=mInv; y*=mInv; return(*this); } VectorR2 operator- () const { return ( VectorR2(-x, -y) ); } @@ -108,7 +108,7 @@ public: VectorR2& MakeUnit(); // Normalize() with error checking VectorR2& ReNormalize(); bool IsUnit( double tolerance = 1.0e-15 ) const - { register double norm = Norm(); + { double norm = Norm(); return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); } bool IsZero() const { return ( x==0.0 && y==0.0 ); } bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );} @@ -369,7 +369,7 @@ inline VectorR2& VectorR2::MakeUnit () // Convert to unit vector (or leave zero inline VectorR2& VectorR2::ReNormalize() // Convert near unit back to unit { double nSq = NormSq(); - register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor + double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor *this *= mFact; return *this; } @@ -395,7 +395,7 @@ inline VectorR2& VectorR2::Rotate( double costheta, double sintheta ) inline double VectorR2::MaxAbs() const { - register double m; + double m; m = (x>=0.0) ? x : -x; if ( y>m ) m=y; else if ( -y >m ) m = -y; @@ -410,17 +410,17 @@ inline VectorR2 operator-( const VectorR2& u, const VectorR2& v ) { return VectorR2(u.x-v.x, u.y-v.y ); } -inline VectorR2 operator*( const VectorR2& u, register double m) +inline VectorR2 operator*( const VectorR2& u, double m) { return VectorR2( u.x*m, u.y*m ); } -inline VectorR2 operator*( register double m, const VectorR2& u) +inline VectorR2 operator*( double m, const VectorR2& u) { return VectorR2( u.x*m, u.y*m ); } inline VectorR2 operator/( const VectorR2& u, double m) { - register double mInv = 1.0/m; + double mInv = 1.0/m; return VectorR2( u.x*mInv, u.y*mInv ); } @@ -454,7 +454,7 @@ inline VectorR2::VectorR2( const VectorHgR2& uH ) inline double NormalizeError (const VectorR2& u) { - register double discrepancy; + double discrepancy; discrepancy = u.x*u.x + u.y*u.y - 1.0; if ( discrepancy < 0.0 ) { discrepancy = -discrepancy; @@ -626,7 +626,7 @@ inline double Matrix2x2::Diagonal( int i ) } inline void Matrix2x2::MakeTranspose() // Transposes it. { - register double temp; + double temp; temp = m12; m12 = m21; m21=temp; @@ -647,8 +647,8 @@ inline void Matrix2x2::operator*= (const Matrix2x2& B) // Matrix product inline Matrix2x2& Matrix2x2::ReNormalize() // Re-normalizes nearly orthonormal matrix { - register double alpha = m11*m11+m21*m21; // First column's norm squared - register double beta = m12*m12+m22*m22; // Second column's norm squared + double alpha = m11*m11+m21*m21; // First column's norm squared + double beta = m12*m12+m22*m22; // Second column's norm squared alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor beta = 1.0 - 0.5*(beta-1.0); m11 *= alpha; // Renormalize first column @@ -657,7 +657,7 @@ inline Matrix2x2& Matrix2x2::ReNormalize() // Re-normalizes nearly orthonormal m m22 *= beta; alpha = m11*m12+m21*m22; // Columns' inner product alpha *= 0.5; // times 1/2 - register double temp; + double temp; temp = m11-alpha*m12; // Subtract alpha times other column m12 -= alpha*m11; m11 = temp; @@ -671,8 +671,8 @@ inline Matrix2x2& Matrix2x2::ReNormalize() // Re-normalizes nearly orthonormal m // Mostly intended for diagnostic purposes. inline double NormalizeError( const Matrix2x2& A) { - register double discrepancy; - register double newdisc; + double discrepancy; + double newdisc; discrepancy = A.m11*A.m11 + A.m21*A.m21 -1.0; // First column - inner product - 1 if (discrepancy<0.0) { discrepancy = -discrepancy; @@ -780,7 +780,7 @@ inline LinearMapR2 operator- (const LinearMapR2& A, const LinearMapR2& B) A.m12-B.m12, A.m22-B.m22 ) ); } -inline LinearMapR2& LinearMapR2::operator*= (register double b) +inline LinearMapR2& LinearMapR2::operator*= ( double b) { m11 *= b; m12 *= b; @@ -789,13 +789,13 @@ inline LinearMapR2& LinearMapR2::operator*= (register double b) return ( *this); } -inline LinearMapR2 operator* ( const LinearMapR2& A, register double b) +inline LinearMapR2 operator* ( const LinearMapR2& A, double b) { return( LinearMapR2( A.m11*b, A.m21*b, A.m12*b, A.m22*b ) ); } -inline LinearMapR2 operator* ( register double b, const LinearMapR2& A) +inline LinearMapR2 operator* ( double b, const LinearMapR2& A) { return( LinearMapR2( A.m11*b, A.m21*b, A.m12*b, A.m22*b ) ); @@ -803,13 +803,13 @@ inline LinearMapR2 operator* ( register double b, const LinearMapR2& A) inline LinearMapR2 operator/ ( const LinearMapR2& A, double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return ( A*bInv ); } -inline LinearMapR2& LinearMapR2::operator/= (register double b) +inline LinearMapR2& LinearMapR2::operator/= ( double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return ( *this *= bInv ); } diff --git a/examples/ThirdPartyLibs/BussIK/LinearR3.cpp b/examples/ThirdPartyLibs/BussIK/LinearR3.cpp index c74d2b2b5..f53b91499 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR3.cpp +++ b/examples/ThirdPartyLibs/BussIK/LinearR3.cpp @@ -47,7 +47,7 @@ const Matrix3x4 Matrix3x4::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, double VectorR3::MaxAbs() const { - register double m; + double m; m = (x>0.0) ? x : -x; if ( y>m ) m=y; else if ( -y >m ) m = -y; @@ -124,9 +124,9 @@ VectorR3& VectorR3::RotateUnitInDirection ( const VectorR3& dir) Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix { - register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared - register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared - register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared + double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared + double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared + double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor beta = 1.0 - 0.5*(beta-1.0); gamma = 1.0 - 0.5*(gamma-1.0); @@ -145,7 +145,7 @@ Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix alpha *= 0.5; beta *= 0.5; gamma *= 0.5; - register double temp1, temp2; + double temp1, temp2; temp1 = m11-alpha*m12-beta*m13; // Update row1 temp2 = m12-alpha*m11-gamma*m13; m13 -= beta*m11+gamma*m12; @@ -199,7 +199,7 @@ VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution double sd23 = m31*m12-m11*m32; double sd33 = m11*m22-m21*m12; - register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); double rx = (u.x*sd11 + u.y*sd21 + u.z*sd31)*detInv; double ry = (u.x*sd12 + u.y*sd22 + u.z*sd32)*detInv; @@ -215,9 +215,9 @@ VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix { - register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared - register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared - register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared + double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared + double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared + double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor beta = 1.0 - 0.5*(beta-1.0); gamma = 1.0 - 0.5*(gamma-1.0); @@ -236,7 +236,7 @@ Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix alpha *= 0.5; beta *= 0.5; gamma *= 0.5; - register double temp1, temp2; + double temp1, temp2; temp1 = m11-alpha*m12-beta*m13; // Update row1 temp2 = m12-alpha*m11-gamma*m13; m13 -= beta*m11+gamma*m12; @@ -340,7 +340,7 @@ LinearMapR3 LinearMapR3::Inverse() const // Returns inverse double sd23 = m31*m12-m11*m32; double sd33 = m11*m22-m21*m12; - register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); return( LinearMapR3( sd11*detInv, sd12*detInv, sd13*detInv, sd21*detInv, sd22*detInv, sd23*detInv, @@ -359,7 +359,7 @@ LinearMapR3& LinearMapR3::Invert() // Converts into inverse. double sd23 = m31*m12-m11*m32; double sd33 = m11*m22-m21*m12; - register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); m11 = sd11*detInv; m12 = sd21*detInv; @@ -441,7 +441,7 @@ AffineMapR3 AffineMapR3::Inverse() const // Returns inverse double sd23 = m31*m12-m11*m32; double sd33 = m11*m22-m21*m12; - register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); // Make sd's hold the (transpose of) the inverse of the 3x3 part sd11 *= detInv; @@ -475,7 +475,7 @@ AffineMapR3& AffineMapR3::Invert() // Converts into inverse. double sd23 = m31*m12-m11*m32; double sd33 = m11*m22-m21*m12; - register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); m11 = sd11*detInv; m12 = sd21*detInv; @@ -541,9 +541,9 @@ RotationMapR3& RotationMapR3::Set( const Quaternion& quat ) RotationMapR3& RotationMapR3::Set( const VectorR3& u, double theta ) { assert ( fabs(u.NormSq()-1.0)<2.0e-6 ); - register double c = cos(theta); - register double s = sin(theta); - register double mc = 1.0-c; + double c = cos(theta); + double s = sin(theta); + double mc = 1.0-c; double xmc = u.x*mc; double xymc = xmc*u.y; double xzmc = xmc*u.z; @@ -673,9 +673,9 @@ RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec) RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double theta ) { assert ( fabs(u.NormSq()-1.0)<2.0e-6 ); - register double c = cos(theta); - register double s = sin(theta); - register double mc = 1.0-c; + double c = cos(theta); + double s = sin(theta); + double mc = 1.0-c; double xmc = u.x*mc; double xymc = xmc*u.y; double xzmc = xmc*u.z; diff --git a/examples/ThirdPartyLibs/BussIK/LinearR3.h b/examples/ThirdPartyLibs/BussIK/LinearR3.h index 4b79d4eed..4efd0fcff 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR3.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR3.h @@ -110,7 +110,7 @@ public: VectorR3& operator*= ( double m ) { x*=m; y*=m; z*=m; return(*this); } VectorR3& operator/= ( double m ) - { register double mInv = 1.0/m; + { double mInv = 1.0/m; x*=mInv; y*=mInv; z*=mInv; return(*this); } VectorR3 operator- () const { return ( VectorR3(-x, -y, -z) ); } @@ -130,10 +130,10 @@ public: inline VectorR3& MakeUnit(); // Normalize() with error checking inline VectorR3& ReNormalize(); bool IsUnit( ) const - { register double norm = Norm(); + { double norm = Norm(); return ( 1.000001>=norm && norm>=0.999999 ); } bool IsUnit( double tolerance ) const - { register double norm = Norm(); + { double norm = Norm(); return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); } bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );} // tolerance should be non-negative @@ -646,17 +646,17 @@ inline VectorR3 operator-( const VectorR3& u, const VectorR3& v ) { return VectorR3(u.x-v.x, u.y-v.y, u.z-v.z); } -inline VectorR3 operator*( const VectorR3& u, register double m) +inline VectorR3 operator*( const VectorR3& u, double m) { return VectorR3( u.x*m, u.y*m, u.z*m); } -inline VectorR3 operator*( register double m, const VectorR3& u) +inline VectorR3 operator*( double m, const VectorR3& u) { return VectorR3( u.x*m, u.y*m, u.z*m); } inline VectorR3 operator/( const VectorR3& u, double m) { - register double mInv = 1.0/m; + double mInv = 1.0/m; return VectorR3( u.x*mInv, u.y*mInv, u.z*mInv); } @@ -716,14 +716,14 @@ inline VectorR3::VectorR3( const VectorHgR3& uH ) inline VectorR3& VectorR3::ReNormalize() // Convert near unit back to unit { double nSq = NormSq(); - register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor + double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor *this *= mFact; return *this; } inline double NormalizeError (const VectorR3& u) { - register double discrepancy; + double discrepancy; discrepancy = u.x*u.x + u.y*u.y + u.z*u.z - 1.0; if ( discrepancy < 0.0 ) { discrepancy = -discrepancy; @@ -1000,7 +1000,7 @@ inline double Matrix3x3::Diagonal( int i ) inline void Matrix3x3::MakeTranspose() // Transposes it. { - register double temp; + double temp; temp = m12; m12 = m21; m21=temp; @@ -1515,7 +1515,7 @@ inline LinearMapR3 operator- (const LinearMapR3& A, const Matrix3x3& B) A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) ); } -inline LinearMapR3& LinearMapR3::operator*= (register double b) +inline LinearMapR3& LinearMapR3::operator*= ( double b) { m11 *= b; m12 *= b; @@ -1529,14 +1529,14 @@ inline LinearMapR3& LinearMapR3::operator*= (register double b) return ( *this); } -inline LinearMapR3 operator* ( const LinearMapR3& A, register double b) +inline LinearMapR3 operator* ( const LinearMapR3& A, double b) { return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b, A.m12*b, A.m22*b, A.m32*b, A.m13*b, A.m23*b, A.m33*b ) ); } -inline LinearMapR3 operator* ( register double b, const LinearMapR3& A) +inline LinearMapR3 operator* ( double b, const LinearMapR3& A) { return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b, A.m12*b, A.m22*b, A.m32*b, @@ -1545,15 +1545,15 @@ inline LinearMapR3 operator* ( register double b, const LinearMapR3& A) inline LinearMapR3 operator/ ( const LinearMapR3& A, double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return( LinearMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv, A.m12*bInv, A.m22*bInv, A.m32*bInv, A.m13*bInv, A.m23*bInv, A.m33*bInv ) ); } -inline LinearMapR3& LinearMapR3::operator/= (register double b) +inline LinearMapR3& LinearMapR3::operator/= ( double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return ( *this *= bInv ); } @@ -1730,7 +1730,7 @@ inline AffineMapR3 operator- (const LinearMapR3& B, const AffineMapR3& A) } -inline AffineMapR3& AffineMapR3::operator*= (register double b) +inline AffineMapR3& AffineMapR3::operator*= ( double b) { m11 *= b; m12 *= b; @@ -1747,7 +1747,7 @@ inline AffineMapR3& AffineMapR3::operator*= (register double b) return (*this); } -inline AffineMapR3 operator* (const AffineMapR3& A, register double b) +inline AffineMapR3 operator* (const AffineMapR3& A, double b) { return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b, A.m12*b, A.m22*b, A.m32*b, @@ -1755,7 +1755,7 @@ inline AffineMapR3 operator* (const AffineMapR3& A, register double b) A.m14*b, A.m24*b, A.m34*b ) ); } -inline AffineMapR3 operator* (register double b, const AffineMapR3& A) +inline AffineMapR3 operator* ( double b, const AffineMapR3& A) { return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b, A.m12*b, A.m22*b, A.m32*b, @@ -1765,14 +1765,14 @@ inline AffineMapR3 operator* (register double b, const AffineMapR3& A) inline AffineMapR3& AffineMapR3::operator/= (double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; *this *= bInv; return( *this ); } inline AffineMapR3 operator/ (const AffineMapR3& A, double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return(AffineMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv, A.m12*bInv, A.m22*bInv, A.m32*bInv, A.m13*bInv, A.m23*bInv, A.m33*bInv, @@ -1823,7 +1823,7 @@ inline RotationMapR3 RotationMapR3::Inverse() const // Returns inverse inline RotationMapR3& RotationMapR3::Invert() // Converts into inverse. { - register double temp; + double temp; temp = m12; m12 = m21; m21 = temp; @@ -1934,7 +1934,7 @@ inline RigidMapR3& RigidMapR3::Invert() // Converts into inverse. m14 = new14; m24 = new24; - register double temp; + double temp; temp = m12; m12 = m21; m21 = temp; diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.cpp b/examples/ThirdPartyLibs/BussIK/LinearR4.cpp index f662a7d90..3c89674a7 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR4.cpp +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.cpp @@ -45,7 +45,7 @@ const Matrix4x4 Matrix4x4::Identity(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, double VectorR4::MaxAbs() const { - register double m; + double m; m = (x>0.0) ? x : -x; if ( y>m ) m=y; else if ( -y >m ) m = -y; @@ -99,7 +99,7 @@ void Matrix4x4::operator*= (const Matrix4x4& B) // Matrix product inline void ReNormalizeHelper ( double &a, double &b, double &c, double &d ) { - register double scaleF = a*a+b*b+c*c+d*d; // Inner product of Vector-R4 + double scaleF = a*a+b*b+c*c+d*d; // Inner product of Vector-R4 scaleF = 1.0-0.5*(scaleF-1.0); a *= scaleF; b *= scaleF; @@ -212,7 +212,7 @@ LinearMapR4 LinearMapR4::Inverse() const // Returns inverse double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12; - register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14); + double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14); return( LinearMapR4( sd11*detInv, -sd12*detInv, sd13*detInv, -sd14*detInv, -sd21*detInv, sd22*detInv, -sd23*detInv, sd24*detInv, @@ -258,7 +258,7 @@ LinearMapR4& LinearMapR4::Invert() // Converts into inverse. double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12; double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12; - register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14); + double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14); m11 = sd11*detInv; m12 = -sd21*detInv; diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h index b0c542249..7e10a925c 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR4.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h @@ -91,7 +91,7 @@ public: VectorR4& operator*= ( double m ) { x*=m; y*=m; z*=m; w*=m; return(*this); } VectorR4& operator/= ( double m ) - { register double mInv = 1.0/m; + { double mInv = 1.0/m; x*=mInv; y*=mInv; z*=mInv; w*=mInv; return(*this); } VectorR4 operator- () const { return ( VectorR4(-x, -y, -z, -w) ); } @@ -109,10 +109,10 @@ public: inline VectorR4& MakeUnit(); // Normalize() with error checking inline VectorR4& ReNormalize(); bool IsUnit( ) const - { register double norm = Norm(); + { double norm = Norm(); return ( 1.000001>=norm && norm>=0.999999 ); } bool IsUnit( double tolerance ) const - { register double norm = Norm(); + { double norm = Norm(); return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); } bool IsZero() const { return ( x==0.0 && y==0.0 && z==0.0 && w==0.0); } bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );} @@ -428,17 +428,17 @@ inline VectorR4 operator-( const VectorR4& u, const VectorR4& v ) { return VectorR4(u.x-v.x, u.y-v.y, u.z-v.z, u.w-v.w); } -inline VectorR4 operator*( const VectorR4& u, register double m) +inline VectorR4 operator*( const VectorR4& u, double m) { return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m ); } -inline VectorR4 operator*( register double m, const VectorR4& u) +inline VectorR4 operator*( double m, const VectorR4& u) { return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m ); } inline VectorR4 operator/( const VectorR4& u, double m) { - register double mInv = 1.0/m; + double mInv = 1.0/m; return VectorR4( u.x*mInv, u.y*mInv, u.z*mInv, u.w*mInv ); } @@ -486,14 +486,14 @@ inline VectorR4& VectorR4::AddScaled( const VectorR4& u, double s ) inline VectorR4& VectorR4::ReNormalize() // Convert near unit back to unit { double nSq = NormSq(); - register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor + double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor *this *= mFact; return *this; } inline double NormalizeError (const VectorR4& u) { - register double discrepancy; + double discrepancy; discrepancy = u.x*u.x + u.y*u.y + u.z*u.z + u.w*u.w - 1.0; if ( discrepancy < 0.0 ) { discrepancy = -discrepancy; @@ -796,7 +796,7 @@ inline double Matrix4x4::Diagonal( int i ) inline void Matrix4x4::MakeTranspose() // Transposes it. { - register double temp; + double temp; temp = m12; m12 = m21; m21=temp; @@ -924,7 +924,7 @@ inline LinearMapR4 operator- (const LinearMapR4& A, const LinearMapR4& B) A.m14-B.m14, A.m24-B.m24, A.m34-B.m34, A.m44-B.m44 ) ); } -inline LinearMapR4& LinearMapR4::operator*= (register double b) +inline LinearMapR4& LinearMapR4::operator*= ( double b) { m11 *= b; m12 *= b; @@ -945,7 +945,7 @@ inline LinearMapR4& LinearMapR4::operator*= (register double b) return ( *this); } -inline LinearMapR4 operator* ( const LinearMapR4& A, register double b) +inline LinearMapR4 operator* ( const LinearMapR4& A, double b) { return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b, A.m12*b, A.m22*b, A.m32*b, A.m42*b, @@ -953,7 +953,7 @@ inline LinearMapR4 operator* ( const LinearMapR4& A, register double b) A.m14*b, A.m24*b, A.m34*b, A.m44*b) ); } -inline LinearMapR4 operator* ( register double b, const LinearMapR4& A) +inline LinearMapR4 operator* ( double b, const LinearMapR4& A) { return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b, A.m12*b, A.m22*b, A.m32*b, A.m42*b, @@ -963,13 +963,13 @@ inline LinearMapR4 operator* ( register double b, const LinearMapR4& A) inline LinearMapR4 operator/ ( const LinearMapR4& A, double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return ( A*bInv ); } -inline LinearMapR4& LinearMapR4::operator/= (register double b) +inline LinearMapR4& LinearMapR4::operator/= ( double b) { - register double bInv = 1.0/b; + double bInv = 1.0/b; return ( *this *= bInv ); } From e2d29d40488d8fa7a2c01bfda99886d79385f8c6 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 17 Nov 2017 13:33:27 -0800 Subject: [PATCH 07/30] also enable depth and segmentation mask preview window in example browser/pybullet You can disable/enable all preview windows using: p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW,0/1) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0/1) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MASK_PREVIEW,0/1) --- .../SharedMemory/PhysicsServerExample.cpp | 58 ++++++++++++++++++- examples/SharedMemory/SharedMemoryPublic.h | 4 ++ examples/pybullet/pybullet.c | 3 + 3 files changed, 63 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 39766cc48..4f051e42b 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1752,8 +1752,8 @@ void PhysicsServerExample::initPhysics() m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight); - //m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight); - //m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight); + m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight); + m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight); for (int i=0;im_visualizerFlag; int enable = m_multiThreadedHelper->m_visualizerEnable; + if (flag == COV_ENABLE_RGB_BUFFER_PREVIEW) + { + if (enable) + { + if (m_canvasRGBIndex<0) + { + m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight); + } + } else + { + if (m_canvasRGBIndex>=0) + { + m_canvas->destroyCanvas(m_canvasRGBIndex); + m_canvasRGBIndex = -1; + } + } + } + + if (flag == COV_ENABLE_DEPTH_BUFFER_PREVIEW) + { + if (enable) + { + if (m_canvasDepthIndex<0) + { + m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight); + } + } else + { + if (m_canvasDepthIndex>=0) + { + m_canvas->destroyCanvas(m_canvasDepthIndex); + m_canvasDepthIndex = -1; + } + } + } + + if (flag == COV_ENABLE_SEGMENTATION_MARK_PREVIEW) + { + if (enable) + { + if (m_canvasSegMaskIndex<0) + { + m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight); + } + } else + { + if (m_canvasSegMaskIndex>=0) + { + m_canvas->destroyCanvas(m_canvasSegMaskIndex); + m_canvasSegMaskIndex = -1; + } + } + } + if (flag==COV_ENABLE_VR_TELEPORTING) { gEnableTeleporting = (enable!=0); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a1f59ae45..fc8fa9c7c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -600,6 +600,10 @@ enum b3ConfigureDebugVisualizerEnum COV_ENABLE_MOUSE_PICKING, COV_ENABLE_Y_AXIS_UP, COV_ENABLE_TINY_RENDERER, + COV_ENABLE_RGB_BUFFER_PREVIEW, + COV_ENABLE_DEPTH_BUFFER_PREVIEW, + COV_ENABLE_SEGMENTATION_MARK_PREVIEW, + }; enum b3AddUserDebugItemEnum diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index afb3fd895..b11fcdc25 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8209,6 +8209,9 @@ initpybullet(void) PyModule_AddIntConstant(m, "COV_ENABLE_KEYBOARD_SHORTCUTS", COV_ENABLE_KEYBOARD_SHORTCUTS); PyModule_AddIntConstant(m, "COV_ENABLE_MOUSE_PICKING", COV_ENABLE_MOUSE_PICKING); + PyModule_AddIntConstant(m, "COV_ENABLE_RGB_BUFFER_PREVIEW", COV_ENABLE_RGB_BUFFER_PREVIEW); + PyModule_AddIntConstant(m, "COV_ENABLE_DEPTH_BUFFER_PREVIEW", COV_ENABLE_DEPTH_BUFFER_PREVIEW); + PyModule_AddIntConstant(m, "COV_ENABLE_SEGMENTATION_MARK_PREVIEW", COV_ENABLE_SEGMENTATION_MARK_PREVIEW); PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); From a9b59b451137c22335231fcf155b034d2dd4cd7c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 17 Nov 2017 16:14:18 -0800 Subject: [PATCH 08/30] Finally refactored the PhysicsServerCommandProcessor::processCommand switch statements into over 50 separate functions. Over time, some of those functions can be moved into separate plugins, similar to vrSyncPlugin (either statically/dynamically linked plugins or dynamically loaded at run-time) --- .../PhysicsServerCommandProcessor.cpp | 10862 ++++++++-------- .../PhysicsServerCommandProcessor.h | 55 + 2 files changed, 5655 insertions(+), 5262 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0f7652666..44ed1eb88 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3064,5400 +3064,5738 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* return streamSizeInBytes; } -bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) +bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { -// BT_PROFILE("processCommand"); - bool hasStatus = false; - { - { - if (m_data->m_commandLogger) + BT_PROFILE("CMD_STATE_LOGGING"); + + serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; + bool hasStatus = true; + + if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) + { + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS) + { + if (m_data->m_profileTimingLoggingUid<0) { - m_data->m_commandLogger->logCommand(clientCmd); + b3ChromeUtilsStartTimings(); + m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + m_data->m_profileTimingLoggingUid = loggerUid; } - serverStatusOut.m_type = CMD_INVALID_STATUS; - serverStatusOut.m_numDataStreamBytes = 0; - serverStatusOut.m_dataStream = 0; - - //consume the command - switch (clientCmd.m_type) - { - - case CMD_STATE_LOGGING: - { - BT_PROFILE("CMD_STATE_LOGGING"); - - serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; - hasStatus = true; - - if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) - { - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS) - { - if (m_data->m_profileTimingLoggingUid<0) - { - b3ChromeUtilsStartTimings(); - m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName; - int loggerUid = m_data->m_stateLoggersUniqueId++; - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - m_data->m_profileTimingLoggingUid = loggerUid; - } - } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) - { - //if (clientCmd.m_stateLoggingArguments.m_fileName) - { - int loggerUid = m_data->m_stateLoggersUniqueId++; - VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper); - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - } - } + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) + { + //if (clientCmd.m_stateLoggingArguments.m_fileName) + { + int loggerUid = m_data->m_stateLoggersUniqueId++; + VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) - { + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) + { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; - //either provide the minitaur by object unique Id, or search for first multibody with 8 motors... + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + //either provide the minitaur by object unique Id, or search for first multibody with 8 motors... - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) - { - int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body) - { - if (body->m_multiBody) - { - btAlignedObjectArray motorNames; - motorNames.push_back("motor_front_leftR_joint"); - motorNames.push_back("motor_front_leftL_joint"); - motorNames.push_back("motor_back_leftR_joint"); - motorNames.push_back("motor_back_leftL_joint"); - motorNames.push_back("motor_front_rightL_joint"); - motorNames.push_back("motor_front_rightR_joint"); - motorNames.push_back("motor_back_rightL_joint"); - motorNames.push_back("motor_back_rightR_joint"); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) + { + int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body) + { + if (body->m_multiBody) + { + btAlignedObjectArray motorNames; + motorNames.push_back("motor_front_leftR_joint"); + motorNames.push_back("motor_front_leftL_joint"); + motorNames.push_back("motor_back_leftR_joint"); + motorNames.push_back("motor_back_leftL_joint"); + motorNames.push_back("motor_front_rightL_joint"); + motorNames.push_back("motor_front_rightR_joint"); + motorNames.push_back("motor_back_rightL_joint"); + motorNames.push_back("motor_back_rightR_joint"); - btAlignedObjectArray motorIdList; - for (int m=0;mm_multiBody->getNumLinks();i++) - { - std::string jointName; - if (body->m_multiBody->getLink(i).m_jointName) - { - jointName = body->m_multiBody->getLink(i).m_jointName; - } - if (motorNames[m]==jointName) - { - motorIdList.push_back(i); - } - } - } - - if (motorIdList.size()==8) - { - int loggerUid = m_data->m_stateLoggersUniqueId++; - MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList); - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - } - } + btAlignedObjectArray motorIdList; + for (int m=0;mm_multiBody->getNumLinks();i++) + { + std::string jointName; + if (body->m_multiBody->getLink(i).m_jointName) + { + jointName = body->m_multiBody->getLink(i).m_jointName; + } + if (motorNames[m]==jointName) + { + motorIdList.push_back(i); } } } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) - { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; - - int loggerUid = m_data->m_stateLoggersUniqueId++; - int maxLogDof = 12; - if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) - { - maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; - } - - int logFlags = 0; - if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS) - { - logFlags = clientCmd.m_stateLoggingArguments.m_logFlags; - } - GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof, logFlags); - - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) - { - logger->m_filterObjectUniqueId = true; - for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) - { - int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; - logger->m_bodyIdList.push_back(objectUniqueId); - } - } - - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + + if (motorIdList.size()==8) { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; int loggerUid = m_data->m_stateLoggersUniqueId++; - ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) - { - logger->m_filterLinkA = true; - logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) - { - logger->m_filterLinkB = true; - logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) - { - logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) - { - logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; - } + MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList); m_data->m_stateLoggers.push_back(logger); serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; } - if (clientCmd.m_stateLoggingArguments.m_logType ==STATE_LOGGING_VR_CONTROLLERS) - { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; - int loggerUid = m_data->m_stateLoggersUniqueId++; - int deviceFilterType = VR_DEVICE_CONTROLLER; - if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE) - { - deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType; - } - VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid,deviceFilterType, fileName); - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + } + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + + int loggerUid = m_data->m_stateLoggersUniqueId++; + int maxLogDof = 12; + if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) + { + maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; + } - } - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0) - { - if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) - { - serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; - b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); - m_data->m_profileTimingLoggingUid = -1; - } - else - { - serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; - for (int i=0;im_stateLoggers.size();i++) - { - if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId) - { - m_data->m_stateLoggers[i]->stop(); - delete m_data->m_stateLoggers[i]; - m_data->m_stateLoggers.removeAtIndex(i); - } - } - } - } - break; + int logFlags = 0; + if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS) + { + logFlags = clientCmd.m_stateLoggingArguments.m_logFlags; + } + GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof, logFlags); + + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) + { + logger->m_filterObjectUniqueId = true; + for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) + { + int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; + logger->m_bodyIdList.push_back(objectUniqueId); + } + } + + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) + { + logger->m_filterLinkA = true; + logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) + { + logger->m_filterLinkB = true; + logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) + { + logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) + { + logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; + } + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + if (clientCmd.m_stateLoggingArguments.m_logType ==STATE_LOGGING_VR_CONTROLLERS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + int deviceFilterType = VR_DEVICE_CONTROLLER; + if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE) + { + deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType; + } + VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid,deviceFilterType, fileName); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + + } + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0) + { + if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); + m_data->m_profileTimingLoggingUid = -1; + } + else + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + for (int i=0;im_stateLoggers.size();i++) + { + if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId) + { + m_data->m_stateLoggers[i]->stop(); + delete m_data->m_stateLoggers[i]; + m_data->m_stateLoggers.removeAtIndex(i); } - case CMD_SET_VR_CAMERA_STATE: + } + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA"); + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; + int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth; + int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; + int numPixelsCopied = 0; + + + + if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) + { + m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, + clientCmd.m_requestPixelDataArguments.m_pixelHeight); + } + + + int numTotalPixels = width*height; + int numRemainingPixels = numTotalPixels - startPixelIndex; + + + if (numRemainingPixels>0) + { + int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask + int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1; + unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; + int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); + + float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); + int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8); + + serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; + float viewMat[16]; + float projMat[16]; + for (int i=0;i<16;i++) + { + viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; + projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; + } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)==0) + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + for (int i=0;i<16;i++) { - BT_PROFILE("CMD_SET_VR_CAMERA_STATE"); - - if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION) - { - gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0]; - gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1]; - gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2]; - } - if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION) - { - gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0]; - gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1]; - gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2]; - gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3]; - } - - if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT) - { - gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; - } - - if (clientCmd.m_updateFlags & VR_CAMERA_FLAG) - { - gVRTrackingObjectFlag = clientCmd.m_vrCameraStateArguments.m_trackingObjectFlag; - } - - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; + viewMat[i] = tmpCamResult.m_viewMatrix[i]; + projMat[i] = tmpCamResult.m_projectionMatrix[i]; } - case CMD_REQUEST_VR_EVENTS_DATA: - { - //BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA"); - serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; - - for (int i=0;im_vrControllerEvents.m_vrEvents[i]; - - if (clientCmd.m_updateFlags&event.m_deviceType) - { - if (event.m_numButtonEvents + event.m_numMoveEvents) - { - serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event; - event.m_numButtonEvents = 0; - event.m_numMoveEvents = 0; - for (int b=0;bm_mouseEvents.size(); - if (serverStatusOut.m_sendMouseEvents.m_numMouseEvents>MAX_MOUSE_EVENTS) - { - serverStatusOut.m_sendMouseEvents.m_numMouseEvents = MAX_MOUSE_EVENTS; - } - for (int i=0;im_mouseEvents[i]; - } - - m_data->m_mouseEvents.resize(0); - serverStatusOut.m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED; - hasStatus = true; - break; - }; - - - - case CMD_REQUEST_KEYBOARD_EVENTS_DATA: - { - //BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA"); - - serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size(); - if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS) - { - serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = MAX_KEYBOARD_EVENTS; - } - for (int i=0;im_keyboardEvents[i]; - } - - btAlignedObjectArray events; - - //remove out-of-date events - for (int i=0;im_keyboardEvents.size();i++) - { - b3KeyboardEvent event = m_data->m_keyboardEvents[i]; - if (event.m_keyState & eButtonIsDown) - { - event.m_keyState = eButtonIsDown; - events.push_back(event); - } - } - m_data->m_keyboardEvents.resize(events.size()); - for (int i=0;im_keyboardEvents[i] = events[i]; - } - - serverStatusOut.m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED; - hasStatus = true; - break; - }; - - case CMD_REQUEST_RAY_CAST_INTERSECTIONS: - { - BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); - serverStatusOut.m_raycastHits.m_numRaycastHits = 0; - - for (int ray=0;raym_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); - int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits; - - if (rayResultCallback.hasHit()) - { - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction - = rayResultCallback.m_closestHitFraction; - - int objectUniqueId = -1; - int linkIndex = -1; - - const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); - if (body) - { - objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); - } else - { - const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject); - if (mblB && mblB->m_multiBody) - { - linkIndex = mblB->m_link; - objectUniqueId = mblB->m_multiBody->getUserIndex2(); - } - } - - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId - = objectUniqueId; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex - = linkIndex; - - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] - = rayResultCallback.m_hitPointWorld[0]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] - = rayResultCallback.m_hitPointWorld[1]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] - = rayResultCallback.m_hitPointWorld[2]; - - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] - = rayResultCallback.m_hitNormalWorld[0]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] - = rayResultCallback.m_hitNormalWorld[1]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] - = rayResultCallback.m_hitNormalWorld[2]; - - } else - { - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1; - serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1; - serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0; - } - serverStatusOut.m_raycastHits.m_numRaycastHits++; - } - serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; - hasStatus = true; - break; - }; - case CMD_REQUEST_DEBUG_LINES: - { - BT_PROFILE("CMD_REQUEST_DEBUG_LINES"); - - int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); - - int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; - int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; - if (startingLineIndex<0) - { - b3Warning("startingLineIndex should be non-negative"); - startingLineIndex = 0; - } - - if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) - { - m_data->m_remoteDebugDrawer->m_lines2.resize(0); - //|btIDebugDraw::DBG_DrawAabb| - // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; - m_data->m_remoteDebugDrawer->setDebugMode(debugMode); - btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer(); - m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer); - m_data->m_dynamicsWorld->debugDrawWorld(); - m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer); - m_data->m_remoteDebugDrawer->setDebugMode(curFlags); - } - - //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' - int bytesPerLine = (sizeof(float) * 9); - int maxNumLines = bufferSizeInBytes/bytesPerLine-1; - if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size()) - { - b3Warning("m_startingLineIndex exceeds total number of debug lines"); - startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); - } - - int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); - - if (numLines) - { - - float* linesFrom = (float*)bufferServerToClient; - float* linesTo = (float*)(bufferServerToClient+numLines*3*sizeof(float)); - float* linesColor = (float*)(bufferServerToClient+2*numLines*3*sizeof(float)); - - for (int i=0;im_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.x(); - linesTo[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.x(); - linesColor[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.x(); - - linesFrom[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.y(); - linesTo[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.y(); - linesColor[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.y(); - - linesFrom[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.z(); - linesTo[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.z(); - linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); - } - } - - serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; - serverStatusOut.m_numDataStreamBytes = numLines * bytesPerLine; - serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; - serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; - serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); - hasStatus = true; - - break; - } - - case CMD_REQUEST_CAMERA_IMAGE_DATA: - { - BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA"); - int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; - int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth; - int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; - int numPixelsCopied = 0; - - - - if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && - (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) - { - m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, - clientCmd.m_requestPixelDataArguments.m_pixelHeight); - } - - - int numTotalPixels = width*height; - int numRemainingPixels = numTotalPixels - startPixelIndex; - - - if (numRemainingPixels>0) - { - int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask - int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1; - unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; - int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); - - float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); - int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8); - - serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; - float viewMat[16]; - float projMat[16]; - for (int i=0;i<16;i++) - { - viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; - projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; - } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)==0) - { - b3OpenGLVisualizerCameraInfo tmpCamResult; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &tmpCamResult.m_width, - &tmpCamResult.m_height, - tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix, - tmpCamResult.m_camUp, - tmpCamResult.m_camForward, - tmpCamResult.m_horizontal, - tmpCamResult.m_vertical, - &tmpCamResult.m_yaw, - &tmpCamResult.m_pitch, - &tmpCamResult.m_dist, - tmpCamResult.m_target); - if (result) - { - for (int i=0;i<16;i++) - { - viewMat[i] = tmpCamResult.m_viewMatrix[i]; - projMat[i] = tmpCamResult.m_projectionMatrix[i]; - } - } - } - bool handled = false; + } + } + bool handled = false; - if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) - { + if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) + { - m_data->m_guiHelper->copyCameraImageData(viewMat, - projMat,pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - segmentationMaskBuffer, numRequestedPixels, - startPixelIndex,width,height,&numPixelsCopied); + m_data->m_guiHelper->copyCameraImageData(viewMat, + projMat,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); - if (numPixelsCopied>0) - { - handled = true; - m_data->m_guiHelper->debugDisplayCameraImageData(viewMat, - projMat,pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - 0, numRequestedPixels, - startPixelIndex,width,height,&numPixelsCopied); - } + if (numPixelsCopied>0) + { + handled = true; + m_data->m_guiHelper->debugDisplayCameraImageData(viewMat, + projMat,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + 0, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); + } - } - if (!handled) - { + } + if (!handled) + { - if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) - { - // printf("-------------------------------\nRendering\n"); + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) + { + // printf("-------------------------------\nRendering\n"); - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) - { - m_data->m_visualConverter.setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) + { + m_data->m_visualConverter.setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) - { - m_data->m_visualConverter.setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) + { + m_data->m_visualConverter.setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) - { - m_data->m_visualConverter.setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) + { + m_data->m_visualConverter.setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) - { - m_data->m_visualConverter.setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow!=0)); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) + { + m_data->m_visualConverter.setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow!=0)); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) - { - m_data->m_visualConverter.setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) + { + m_data->m_visualConverter.setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) - { - m_data->m_visualConverter.setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) + { + m_data->m_visualConverter.setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) - { - m_data->m_visualConverter.setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) - { - m_data->m_visualConverter.render( - clientCmd.m_requestPixelDataArguments.m_viewMatrix, - clientCmd.m_requestPixelDataArguments.m_projectionMatrix); - } else - { - b3OpenGLVisualizerCameraInfo tmpCamResult; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &tmpCamResult.m_width, - &tmpCamResult.m_height, - tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix, - tmpCamResult.m_camUp, - tmpCamResult.m_camForward, - tmpCamResult.m_horizontal, - tmpCamResult.m_vertical, - &tmpCamResult.m_yaw, - &tmpCamResult.m_pitch, - &tmpCamResult.m_dist, - tmpCamResult.m_target); - if (result) - { - m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix); - } else - { - m_data->m_visualConverter.render(); - } - } - } - - m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - segmentationMaskBuffer, numRequestedPixels, - startPixelIndex,&width,&height,&numPixelsCopied); - - m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, - clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - segmentationMaskBuffer, numRequestedPixels, - startPixelIndex,width,height,&numPixelsCopied); - - } - - //each pixel takes 4 RGBA values and 1 float = 8 bytes - - } else - { - - } - - serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; - - serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; - serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; - serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex; - serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; - serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; - hasStatus = true; - - break; + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) + { + m_data->m_visualConverter.setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); } - case CMD_SYNC_BODY_INFO: + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) { - BT_PROFILE("CMD_SYNC_BODY_INFO"); - - b3AlignedObjectArray usedHandles; - m_data->m_bodyHandles.getUsedHandles(usedHandles); - int actualNumBodies = 0; - for (int i=0;im_visualConverter.render( + clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix); + } else + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) { - int usedHandle = usedHandles[i]; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle); - if (body && (body->m_multiBody || body->m_rigidBody)) - { - serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; - } - } - serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; - - int usz = m_data->m_userConstraints.size(); - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; - for (int i=0;im_visualConverter.render(tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix); + } else { - - int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1(); -// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId; - serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key; + m_data->m_visualConverter.render(); } - - serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; - hasStatus = true; - break; } - case CMD_REQUEST_BODY_INFO: - { - BT_PROFILE("CMD_REQUEST_BODY_INFO"); + } - const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; - //stream info into memory - int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,&width,&height,&numPixelsCopied); - serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; - serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0; - - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(sdfInfoArgs.m_bodyUniqueId); - if (bodyHandle) - { - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,bodyHandle->m_bodyName.c_str()); - } + m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; - - hasStatus = true; - break; - } - case CMD_SAVE_WORLD: - { - BT_PROFILE("CMD_SAVE_WORLD"); + } - ///this is a very rudimentary way to save the state of the world, for scene authoring - ///many todo's, for example save the state of motor controllers etc. + //each pixel takes 4 RGBA values and 1 float = 8 bytes + + } else + { + + } + + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; + serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex; + serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; + serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = false; + BT_PROFILE("CMD_SAVE_WORLD"); + serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED; + + ///this is a very rudimentary way to save the state of the world, for scene authoring + ///many todo's, for example save the state of motor controllers etc. + + { + //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName); + int constraintCount = 0; + FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w"); + if (f) + { + char line[1024]; + { + sprintf(line,"import pybullet as p\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + { + sprintf(line,"cin = p.connect(p.SHARED_MEMORY)\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + { + sprintf(line,"if (cin < 0):\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + { + sprintf(line," cin = p.connect(p.GUI)\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + + //for each objects ... + for (int i=0;im_saveWorldBodyData.size();i++) + { + SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i]; + + for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); + if (body) { - char line[1024]; - { - sprintf(line,"import pybullet as p\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line,"cin = p.connect(p.SHARED_MEMORY)\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line,"if (cin < 0):\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line," cin = p.connect(p.GUI)\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - - //for each objects ... - for (int i=0;im_saveWorldBodyData.size();i++) - { - SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i]; - - for (int i=0;im_multiBody) { - { - int bodyUniqueId = sd.m_bodyUniqueIds[i]; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body) - { - if (body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; + btMultiBody* mb = body->m_multiBody; - btTransform comTr = mb->getBaseWorldTransform(); - btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse(); + btTransform comTr = mb->getBaseWorldTransform(); + btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse(); - if (strstr(sd.m_fileName.c_str(),".urdf")) - { - sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(), - tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2], - tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (strstr(sd.m_fileName.c_str(),".sdf") && i==0) - { - sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str()); - int len = strlen(line); - fwrite(line,len,1,f); - } - if (strstr(sd.m_fileName.c_str(),".xml") && i==0) - { - sprintf(line,"objects = p.loadMJCF(\"%s\")\n",sd.m_fileName.c_str()); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (strstr(sd.m_fileName.c_str(),".sdf") || strstr(sd.m_fileName.c_str(),".xml") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) ) - { - sprintf(line,"ob = objects[%d]\n",i); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (strstr(sd.m_fileName.c_str(),".sdf")||strstr(sd.m_fileName.c_str(),".xml")) - { - sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n", - comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2], - comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (mb->getNumLinks()) - { - { - sprintf(line,"jointPositions=["); - int len = strlen(line); - fwrite(line,len,1,f); - } - - for (int i=0;igetNumLinks();i++) - { - btScalar jointPos = mb->getJointPosMultiDof(i)[0]; - if (igetNumLinks()-1) - { - sprintf(line," %f,",jointPos); - int len = strlen(line); - fwrite(line,len,1,f); - } else - { - sprintf(line," %f ",jointPos); - int len = strlen(line); - fwrite(line,len,1,f); - } - } - - { - sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - } - } else - { - //todo: btRigidBody/btSoftBody etc case - } - } - } - - } - - //for URDF, load at origin, then reposition... - - - struct SaveWorldObjectData + if (strstr(sd.m_fileName.c_str(),".urdf")) { - b3AlignedObjectArray m_bodyUniqueIds; - std::string m_fileName; - }; - } - - //user constraints - { - for (int i=0;im_userConstraints.size();i++) - { - InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i); - b3UserConstraint& uc = ucptr->m_userConstraintData; - - int parentBodyIndex=uc.m_parentBodyIndex; - int parentJointIndex=uc.m_parentJointIndex; - int childBodyIndex=uc.m_childBodyIndex; - int childJointIndex=uc.m_childJointIndex; - btVector3 jointAxis(uc.m_jointAxis[0],uc.m_jointAxis[1],uc.m_jointAxis[2]); - btVector3 pivotParent(uc.m_parentFrame[0],uc.m_parentFrame[1],uc.m_parentFrame[2]); - btVector3 pivotChild(uc.m_childFrame[0],uc.m_childFrame[1],uc.m_childFrame[2]); - btQuaternion ornFrameParent(uc.m_parentFrame[3],uc.m_parentFrame[4],uc.m_parentFrame[5],uc.m_parentFrame[6]); - btQuaternion ornFrameChild(uc.m_childFrame[3],uc.m_childFrame[4],uc.m_childFrame[5],uc.m_childFrame[6]); - { - char jointTypeStr[1024]="FIXED"; - bool hasKnownJointType = true; - - switch (uc.m_jointType) - { - case eRevoluteType: - { - sprintf(jointTypeStr,"p.JOINT_REVOLUTE"); - break; - } - case ePrismaticType: - { - sprintf(jointTypeStr,"p.JOINT_PRISMATIC"); - break; - } - case eSphericalType: - { - sprintf(jointTypeStr,"p.JOINT_SPHERICAL"); - break; - } - case ePlanarType: - { - sprintf(jointTypeStr,"p.JOINT_PLANAR"); - break; - } - case eFixedType : - { - sprintf(jointTypeStr,"p.JOINT_FIXED"); - break; - } - case ePoint2PointType: - { - sprintf(jointTypeStr,"p.JOINT_POINT2POINT"); - break; - } - case eGearType: - { - sprintf(jointTypeStr,"p.JOINT_GEAR"); - break; - } - default: - { - hasKnownJointType = false; - b3Warning("unknown constraint type in SAVE_WORLD"); - } - }; - if (hasKnownJointType) - { - { - sprintf(line,"cid%d = p.createConstraint(%d,%d,%d,%d,%s,[%f,%f,%f],[%f,%f,%f],[%f,%f,%f],[%f,%f,%f,%f],[%f,%f,%f,%f])\n", - constraintCount, - parentBodyIndex, - parentJointIndex, - childBodyIndex, - childJointIndex, - jointTypeStr, - jointAxis[0],jointAxis[1],jointAxis[2], - pivotParent[0],pivotParent[1],pivotParent[2], - pivotChild[0],pivotChild[1],pivotChild[2], - ornFrameParent[0],ornFrameParent[1],ornFrameParent[2],ornFrameParent[3], - ornFrameChild[0],ornFrameChild[1],ornFrameChild[2],ornFrameChild[3] - ); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line,"p.changeConstraint(cid%d,maxForce=%f)\n",constraintCount,uc.m_maxAppliedForce); - int len = strlen(line); - fwrite(line,len,1,f); - constraintCount++; - } - } - } - } - } - - { - btVector3 grav=this->m_data->m_dynamicsWorld->getGravity(); - sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); - int len = strlen(line); - fwrite(line,len,1,f); - } - - - { - sprintf(line,"p.stepSimulation()\np.disconnect()\n"); + sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(), + tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2], + tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]); int len = strlen(line); fwrite(line,len,1,f); - } - fclose(f); + } + + if (strstr(sd.m_fileName.c_str(),".sdf") && i==0) + { + sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str()); + int len = strlen(line); + fwrite(line,len,1,f); + } + if (strstr(sd.m_fileName.c_str(),".xml") && i==0) + { + sprintf(line,"objects = p.loadMJCF(\"%s\")\n",sd.m_fileName.c_str()); + int len = strlen(line); + fwrite(line,len,1,f); + } + + if (strstr(sd.m_fileName.c_str(),".sdf") || strstr(sd.m_fileName.c_str(),".xml") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) ) + { + sprintf(line,"ob = objects[%d]\n",i); + int len = strlen(line); + fwrite(line,len,1,f); + } + + if (strstr(sd.m_fileName.c_str(),".sdf")||strstr(sd.m_fileName.c_str(),".xml")) + { + sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n", + comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2], + comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]); + int len = strlen(line); + fwrite(line,len,1,f); + } + + if (mb->getNumLinks()) + { + { + sprintf(line,"jointPositions=["); + int len = strlen(line); + fwrite(line,len,1,f); + } + + for (int i=0;igetNumLinks();i++) + { + btScalar jointPos = mb->getJointPosMultiDof(i)[0]; + if (igetNumLinks()-1) + { + sprintf(line," %f,",jointPos); + int len = strlen(line); + fwrite(line,len,1,f); + } else + { + sprintf(line," %f ",jointPos); + int len = strlen(line); + fwrite(line,len,1,f); + } + } + + { + sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + } + } else + { + //todo: btRigidBody/btSoftBody etc case + } } - - - serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED; - hasStatus = true; - break; } - serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED; - hasStatus = true; - break; + } - case CMD_LOAD_SDF: - { - BT_PROFILE("CMD_LOAD_SDF"); + + //for URDF, load at origin, then reposition... + - const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true; - - int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA - btScalar globalScaling = 1.f; - if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) - { - globalScaling = sdfArgs.m_globalScaling; - } - bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling); - if (completedOk) - { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - - //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; - serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; - int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); - for (int i=0;im_sdfRecentLoadedBodies[i]; - } - - serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; - } else - { - serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; - } - hasStatus = true; - break; - } - case CMD_CREATE_COLLISION_SHAPE: + struct SaveWorldObjectData { - hasStatus = true; - serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; - - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + b3AlignedObjectArray m_bodyUniqueIds; + std::string m_fileName; + }; + } - btCollisionShape* shape = 0; - b3AlignedObjectArray urdfCollisionObjects; + //user constraints + { + for (int i=0;im_userConstraints.size();i++) + { + InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i); + b3UserConstraint& uc = ucptr->m_userConstraintData; - btCompoundShape* compound = 0; - - if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1) + int parentBodyIndex=uc.m_parentBodyIndex; + int parentJointIndex=uc.m_parentJointIndex; + int childBodyIndex=uc.m_childBodyIndex; + int childJointIndex=uc.m_childJointIndex; + btVector3 jointAxis(uc.m_jointAxis[0],uc.m_jointAxis[1],uc.m_jointAxis[2]); + btVector3 pivotParent(uc.m_parentFrame[0],uc.m_parentFrame[1],uc.m_parentFrame[2]); + btVector3 pivotChild(uc.m_childFrame[0],uc.m_childFrame[1],uc.m_childFrame[2]); + btQuaternion ornFrameParent(uc.m_parentFrame[3],uc.m_parentFrame[4],uc.m_parentFrame[5],uc.m_parentFrame[6]); + btQuaternion ornFrameChild(uc.m_childFrame[3],uc.m_childFrame[4],uc.m_childFrame[5],uc.m_childFrame[6]); { - compound = worldImporter->createCompoundShape(); - } - for (int i=0;im_data->m_dynamicsWorld->getGravity(); + sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); + int len = strlen(line); + fwrite(line,len,1,f); + } + + + { + sprintf(line,"p.stepSimulation()\np.disconnect()\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + fclose(f); + } + + + serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED; + hasStatus = true; + } + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + + btCollisionShape* shape = 0; + b3AlignedObjectArray urdfCollisionObjects; + + btCompoundShape* compound = 0; + + if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1) + { + compound = worldImporter->createCompoundShape(); + } + for (int i=0;icreateCompoundShape(); + } + } + + urdfColObj.m_linkLocalFrame = childTransform; + urdfColObj.m_sourceFileLocation = "memory"; + urdfColObj.m_name = "memory"; + urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN; + + switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type) + { + case GEOM_SPHERE: + { + double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; + shape = worldImporter->createSphereShape(radius); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE; + urdfColObj.m_geometry.m_sphereRadius = radius; + break; + } + case GEOM_BOX: + { + //double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; + btVector3 halfExtents( + clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]); + shape = worldImporter->createBoxShape(halfExtents); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_BOX; + urdfColObj.m_geometry.m_boxSize = 2.*halfExtents; + break; + } + case GEOM_CAPSULE: + { + shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + + break; + } + case GEOM_CYLINDER: + { + shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + + break; + } + case GEOM_PLANE: + { + btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); + + shape = worldImporter->createPlaneShape(planeNormal,0); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE; + urdfColObj.m_geometry.m_planeNormal.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); + + break; + } + case GEOM_MESH: + { + btScalar defaultCollisionMargin = 0.001; + + btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]); + + const std::string& urdf_path=""; + + std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName; + urdfColObj.m_geometry.m_type = URDF_GEOM_MESH; + urdfColObj.m_geometry.m_meshFileName = fileName; + + urdfColObj.m_geometry.m_meshScale = meshScale; + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + const std::string& error_message_prefix=""; + std::string out_found_filename; + int out_type; + + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); + if (foundFile) + { + urdfColObj.m_geometry.m_meshFileType = out_type; + + if (out_type==UrdfGeometry::FILE_OBJ) + { + //create a convex hull for each shape, and store it in a btCompoundShape + + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) + { + GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix); + + if (!glmesh || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); + delete glmesh; + break; + } + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + + for (int i=0; im_numvertices; i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*meshScale[2])); + } + + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + this->m_data->m_meshInterfaces.push_back(meshInterface); + { + BT_PROFILE("convert vertices"); + + for (int i=0; im_numIndices/3; i++) + { + const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; + const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; + const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; + meshInterface->addTriangle(v0,v1,v2); + } + } + { + BT_PROFILE("create btBvhTriangleMeshShape"); + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + m_data->m_collisionShapes.push_back(trimesh); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + if (compound) + { + compound->addChildShape(childTransform,shape); + } + } + delete glmesh; + } else + { + + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + + //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) + B3_PROFILE("createConvexHullFromShapes"); if (compound==0) { compound = worldImporter->createCompoundShape(); } + compound->setMargin(defaultCollisionMargin); + + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(defaultCollisionMargin); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt*meshScale,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(childTransform,convexHull); + } } + } + } + break; + } + default: + { + } + } + if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN) + { + urdfCollisionObjects.push_back(urdfColObj); + } + } - urdfColObj.m_linkLocalFrame = childTransform; - urdfColObj.m_sourceFileLocation = "memory"; - urdfColObj.m_name = "memory"; - urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN; + if (compound && compound->getNumChildShapes()) + { + shape = compound; + } - switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type) - { - case GEOM_SPHERE: - { - double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; - shape = worldImporter->createSphereShape(radius); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE; - urdfColObj.m_geometry.m_sphereRadius = radius; - break; - } - case GEOM_BOX: - { - //double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; - btVector3 halfExtents( - clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]); - shape = worldImporter->createBoxShape(halfExtents); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_BOX; - urdfColObj.m_geometry.m_boxSize = 2.*halfExtents; - break; - } - case GEOM_CAPSULE: - { - shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, - clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE; - urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; - urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + if (shape) + { + int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); + handle->m_collisionShape = shape; + for (int i=0;im_urdfCollisionObjects.push_back(urdfCollisionObjects[i]); + } + serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid; + m_data->m_worldImporters.push_back(worldImporter); + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; + } else + { + delete worldImporter; + } - break; - } - case GEOM_CYLINDER: - { - shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, - clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER; - urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; - urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + return hasStatus; +} - break; - } - case GEOM_PLANE: - { - btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); +bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; + double globalScaling = 1.f; - shape = worldImporter->createPlaneShape(planeNormal,0); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE; - urdfColObj.m_geometry.m_planeNormal.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); - - break; - } - case GEOM_MESH: - { - btScalar defaultCollisionMargin = 0.001; + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); + u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); + btTransform localInertiaFrame; + localInertiaFrame.setIdentity(); + btTransform childTrans; + childTrans.setIdentity(); + const char* pathPrefix = ""; + if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1) + { + int userShapeIndex = 0; - btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]); + UrdfVisual visualShape; + visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type; + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; - const std::string& urdf_path=""; - - std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName; - urdfColObj.m_geometry.m_type = URDF_GEOM_MESH; - urdfColObj.m_geometry.m_meshFileName = fileName; - - urdfColObj.m_geometry.m_meshScale = meshScale; - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) - { - - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } + if (visualShape.m_geometry.m_type == URDF_GEOM_MESH) + { - const std::string& error_message_prefix=""; - std::string out_found_filename; - int out_type; + std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName; + const std::string& error_message_prefix=""; + std::string out_found_filename; + int out_type; + if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); + visualShape.m_geometry.m_meshFileType = out_type; + visualShape.m_geometry.m_meshFileName=fileName; - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); - if (foundFile) + visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]); + } + visualShape.m_name = "bla"; + visualShape.m_materialName=""; + visualShape.m_sourceFileLocation="blaat_line_10"; + visualShape.m_linkLocalFrame.setIdentity(); + visualShape.m_geometry.m_hasLocalMaterial = false; + + + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + btAlignedObjectArray textures; + bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR)!=0;; + bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR)!=0;; + visualShape.m_geometry.m_hasLocalMaterial = hasRGBA|hasSpecular; + if (visualShape.m_geometry.m_hasLocalMaterial) + { + if (hasRGBA) + { + visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]); + } else + { + + } + if (hasSpecular) + { + visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]); + } else + { + visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4,0.4,0.4); + } + } + + if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform !=0) + { + childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[2])); + childTrans.setRotation(btQuaternion( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[2], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3])); + } + + + + u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); + + if (vertices.size() && indices.size()) + { + if (1) + { + int textureIndex = -1; + if (textures.size()) + { + + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1,textures[0].m_width,textures[0].m_height); + } + int graphicsIndex = -1; + { + B3_PROFILE("registerGraphicsShape"); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + if (graphicsIndex>=0) + { + int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); + InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); + visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; + visualHandle->m_tinyRendererVisualShapeIndex = -1; + //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance + //store needed info for tinyrenderer + visualHandle->m_localInertiaFrame = localInertiaFrame; + visualHandle->m_visualShape = visualShape; + visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; + + serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; + } + } + } + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; + + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) + { + //pluginPath could be registered or load from disk + const char* postFix = ""; + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX) + { + postFix = clientCmd.m_customCommandArgs.m_postFix; + } + + int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix); + if (pluginUniqueId>=0) + { + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN) + { + m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId); + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND) + { + + int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); + serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_USER_DEBUG_DRAW"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; + + + int trackingVisualShapeIndex = -1; + + if (clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId>=0) + { + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId); + if (bodyHandle) + { + int linkIndex = -1; + + if (bodyHandle->m_multiBody) + { + int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; + if (linkIndex ==-1) + { + if (bodyHandle->m_multiBody->getBaseCollider()) + { + trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + } + } else + { + if (linkIndex >=0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) + { + if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) + { + trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + } + } + + } + } + if (bodyHandle->m_rigidBody) + { + trackingVisualShapeIndex = bodyHandle->m_rigidBody->getUserIndex(); + } + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) + { + int uid = m_data->m_guiHelper->addUserDebugParameter( + clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_rangeMin, + clientCmd.m_userDebugDrawArgs.m_rangeMax, + clientCmd.m_userDebugDrawArgs.m_startValue + ); + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags &USER_DEBUG_READ_PARAMETER) + { + + int ok = m_data->m_guiHelper->readUserDebugParameter( + clientCmd.m_userDebugDrawArgs.m_itemUniqueId, + &serverCmd.m_userDebugDrawArgs.m_parameterValue); + if (ok) + { + serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; + } + } + if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) + { + int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body) + { + btCollisionObject* destColObj = 0; + + if (body->m_multiBody) + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1) + { + destColObj = body->m_multiBody->getBaseCollider(); + } + else + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks()) + { + destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider; + } + } + + } + if (body->m_rigidBody) + { + destColObj = body->m_rigidBody; + } + + if (destColObj) + { + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR) + { + destColObj->removeCustomDebugColor(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) + { + btVector3 objectColorRGB; + objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]); + destColObj->setCustomDebugColor(objectColorRGB); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) + { + //addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;} + + int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags; + + if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION)==0) + { + optionFlags |= DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA; + } + + + int replaceItemUniqueId = -1; + if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)!=0) + { + replaceItemUniqueId = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId; + } + + + int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, + clientCmd.m_userDebugDrawArgs.m_textOrientation, + clientCmd.m_userDebugDrawArgs.m_textColorRGB, + clientCmd.m_userDebugDrawArgs.m_textSize, + clientCmd.m_userDebugDrawArgs.m_lifeTime, + trackingVisualShapeIndex, + optionFlags, + replaceItemUniqueId); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) + { + int uid = m_data->m_guiHelper->addUserDebugLine( + clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, + clientCmd.m_userDebugDrawArgs.m_lineWidth, + clientCmd.m_userDebugDrawArgs.m_lifeTime, + trackingVisualShapeIndex); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) + { + m_data->m_guiHelper->removeAllUserDebugItems(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) + { + m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SET_VR_CAMERA_STATE"); + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION) + { + gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0]; + gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1]; + gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2]; + } + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION) + { + gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0]; + gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1]; + gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2]; + gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3]; + } + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT) + { + gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; + } + + if (clientCmd.m_updateFlags & VR_CAMERA_FLAG) + { + gVRTrackingObjectFlag = clientCmd.m_vrCameraStateArguments.m_trackingObjectFlag; + } + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestVREventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + //BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA"); + serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; + + for (int i=0;im_vrControllerEvents.m_vrEvents[i]; + + if (clientCmd.m_updateFlags&event.m_deviceType) + { + if (event.m_numButtonEvents + event.m_numMoveEvents) + { + serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event; + event.m_numButtonEvents = 0; + event.m_numMoveEvents = 0; + for (int b=0;bm_mouseEvents.size(); + if (serverStatusOut.m_sendMouseEvents.m_numMouseEvents>MAX_MOUSE_EVENTS) + { + serverStatusOut.m_sendMouseEvents.m_numMouseEvents = MAX_MOUSE_EVENTS; + } + for (int i=0;im_mouseEvents[i]; + } + + m_data->m_mouseEvents.resize(0); + serverStatusOut.m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestKeyboardEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + //BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA"); + bool hasStatus = true; + serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size(); + if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS) + { + serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = MAX_KEYBOARD_EVENTS; + } + for (int i=0;im_keyboardEvents[i]; + } + + btAlignedObjectArray events; + + //remove out-of-date events + for (int i=0;im_keyboardEvents.size();i++) + { + b3KeyboardEvent event = m_data->m_keyboardEvents[i]; + if (event.m_keyState & eButtonIsDown) + { + event.m_keyState = eButtonIsDown; + events.push_back(event); + } + } + m_data->m_keyboardEvents.resize(events.size()); + for (int i=0;im_keyboardEvents[i] = events[i]; + } + + serverStatusOut.m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); + serverStatusOut.m_raycastHits.m_numRaycastHits = 0; + + for (int ray=0;raym_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); + int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits; + + if (rayResultCallback.hasHit()) + { + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction + = rayResultCallback.m_closestHitFraction; + + int objectUniqueId = -1; + int linkIndex = -1; + + const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); + if (body) + { + objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); + } else + { + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject); + if (mblB && mblB->m_multiBody) + { + linkIndex = mblB->m_link; + objectUniqueId = mblB->m_multiBody->getUserIndex2(); + } + } + + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId + = objectUniqueId; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex + = linkIndex; + + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] + = rayResultCallback.m_hitPointWorld[0]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] + = rayResultCallback.m_hitPointWorld[1]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] + = rayResultCallback.m_hitPointWorld[2]; + + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] + = rayResultCallback.m_hitNormalWorld[0]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] + = rayResultCallback.m_hitNormalWorld[1]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] + = rayResultCallback.m_hitNormalWorld[2]; + + } else + { + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0; + } + serverStatusOut.m_raycastHits.m_numRaycastHits++; + } + serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestDebugLinesCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_DEBUG_LINES"); + int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); + + int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; + int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; + if (startingLineIndex<0) + { + b3Warning("startingLineIndex should be non-negative"); + startingLineIndex = 0; + } + + if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) + { + m_data->m_remoteDebugDrawer->m_lines2.resize(0); + //|btIDebugDraw::DBG_DrawAabb| + // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; + m_data->m_remoteDebugDrawer->setDebugMode(debugMode); + btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer(); + m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer); + m_data->m_dynamicsWorld->debugDrawWorld(); + m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer); + m_data->m_remoteDebugDrawer->setDebugMode(curFlags); + } + + //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' + int bytesPerLine = (sizeof(float) * 9); + int maxNumLines = bufferSizeInBytes/bytesPerLine-1; + if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size()) + { + b3Warning("m_startingLineIndex exceeds total number of debug lines"); + startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); + } + + int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); + + if (numLines) + { + + float* linesFrom = (float*)bufferServerToClient; + float* linesTo = (float*)(bufferServerToClient+numLines*3*sizeof(float)); + float* linesColor = (float*)(bufferServerToClient+2*numLines*3*sizeof(float)); + + for (int i=0;im_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.x(); + linesTo[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.x(); + linesColor[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.x(); + + linesFrom[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.y(); + linesTo[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.y(); + linesColor[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.y(); + + linesFrom[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.z(); + linesTo[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.z(); + linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); + } + } + + serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; + serverStatusOut.m_numDataStreamBytes = numLines * bytesPerLine; + serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; + serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; + serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_BODY_INFO"); + + b3AlignedObjectArray usedHandles; + m_data->m_bodyHandles.getUsedHandles(usedHandles); + int actualNumBodies = 0; + for (int i=0;im_bodyHandles.getHandle(usedHandle); + if (body && (body->m_multiBody || body->m_rigidBody)) + { + serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; + } + } + serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; + + int usz = m_data->m_userConstraints.size(); + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; + for (int i=0;im_userConstraints.getKeyAtIndex(i).getUid1(); +// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId; + serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key; + } + + serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SEND_DESIRED_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_SEND_DESIRED_STATE"); + } + + int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + btAssert(mb); + + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) + { + case CONTROL_MODE_TORQUE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_TORQUE"); + } + // mb->clearForcesAndTorques(); + int torqueIndex = 6; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + } + torqueIndex++; + } + } + } + break; + } + case CONTROL_MODE_VELOCITY: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_VELOCITY"); + } + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + + if (motor) + { + btScalar desiredVelocity = 0.f; + bool hasDesiredVelocity = false; + + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) { - urdfColObj.m_geometry.m_meshFileType = out_type; - - if (out_type==UrdfGeometry::FILE_OBJ) + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + btScalar kd = 0.1f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) { - //create a convex hull for each shape, and store it in a btCompoundShape + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; + } - if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) + motor->setVelocityTarget(desiredVelocity,kd); + + btScalar kp = 0.f; + motor->setPositionTarget(0,kp); + hasDesiredVelocity = true; + } + if (hasDesiredVelocity) + { + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + } + motor->setMaxAppliedImpulse(maxImp); + } + numMotors++; + + } + } + dofIndex += mb->getLink(link).m_dofCount; + } + } + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); + } + //compute the force base on PD control + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base + int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + if (motor) + { + + bool hasDesiredPosOrVel = false; + btScalar kp = 0.f; + btScalar kd = 0.f; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + kd = 0.1; + } + btScalar desiredPosition = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + kp = 0.1; + } + + if (hasDesiredPosOrVel) + { + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + } + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + } + + motor->setVelocityTarget(desiredVelocity,kd); + motor->setPositionTarget(desiredPosition,kp); + + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + + motor->setMaxAppliedImpulse(maxImp); + } + numMotors++; + } + + } + velIndex += mb->getLink(link).m_dofCount; + posIndex += mb->getLink(link).m_posVarCount; + } + } + + break; + } + default: + { + b3Warning("m_controlMode not implemented yet"); + break; + } + + } + } else + { + //support for non-btMultiBody, such as btRigidBody + + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + btAssert(rb); + + //switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) + { + //case CONTROL_MODE_TORQUE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_TORQUE"); + } + // mb->clearForcesAndTorques(); + ///see addJointInfoFromConstraint + int velIndex = 6; + int posIndex = 7; + //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + for (int link=0;linkm_rigidBodyJoints.size();link++) + { + btGeneric6DofSpring2Constraint* con = body->m_rigidBodyJoints[link]; + + btVector3 linearLowerLimit; + btVector3 linearUpperLimit; + btVector3 angularLowerLimit; + btVector3 angularUpperLimit; + + + //for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + + + { + + int torqueIndex = velIndex; + double torque = 100; + bool hasDesiredTorque = false; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]; + hasDesiredTorque = true; + } + + bool hasDesiredPosOrVel = false; + btScalar qdotTarget = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + } + btScalar qTarget = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + } + + con->getLinearLowerLimit(linearLowerLimit); + con->getLinearUpperLimit(linearUpperLimit); + con->getAngularLowerLimit(angularLowerLimit); + con->getAngularUpperLimit(angularUpperLimit); + + if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero()) + { + //fixed, don't do anything + } else + { + con->calculateTransforms(); + + if (linearLowerLimit.isZero() && linearUpperLimit.isZero()) { - GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix); - - if (!glmesh || glmesh->m_numvertices<=0) - { - b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); - delete glmesh; - break; - } - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - - for (int i=0; im_numvertices; i++) - { - convertedVerts.push_back(btVector3( - glmesh->m_vertices->at(i).xyzw[0]*meshScale[0], - glmesh->m_vertices->at(i).xyzw[1]*meshScale[1], - glmesh->m_vertices->at(i).xyzw[2]*meshScale[2])); - } + //eRevoluteType; + btVector3 limitRange = angularLowerLimit.absolute()+angularUpperLimit.absolute(); + int limitAxis = limitRange.maxAxis(); + const btTransform& transA = con->getCalculatedTransformA(); + const btTransform& transB = con->getCalculatedTransformB(); + btVector3 axisA = transA.getBasis().getColumn(limitAxis); + btVector3 axisB = transB.getBasis().getColumn(limitAxis); - BT_PROFILE("convert trimesh"); - btTriangleMesh* meshInterface = new btTriangleMesh(); - this->m_data->m_meshInterfaces.push_back(meshInterface); + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { - BT_PROFILE("convert vertices"); + case CONTROL_MODE_TORQUE: + { + if (hasDesiredTorque) + { + con->getRigidBodyA().applyTorque(torque*axisA); + con->getRigidBodyB().applyTorque(-torque*axisB); + } + break; + } + case CONTROL_MODE_VELOCITY: + { + if (hasDesiredPosOrVel) + { + con->enableMotor(3+limitAxis,true); + con->setTargetVelocity(3+limitAxis, qdotTarget); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(3+limitAxis,torqueImpulse); + } + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + if (hasDesiredPosOrVel) + { + con->setServo(3+limitAxis,true); + con->setServoTarget(3+limitAxis,-qTarget); + //next one is the maximum velocity to reach target position. + //the maximum velocity is limited by maxMotorForce + con->setTargetVelocity(3+limitAxis, 100); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(3+limitAxis,torqueImpulse); + con->enableMotor(3+limitAxis,true); + } + break; + } + default: + { + } + }; + + + - for (int i=0; im_numIndices/3; i++) - { - const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; - const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; - const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; - meshInterface->addTriangle(v0,v1,v2); - } - } - { - BT_PROFILE("create btBvhTriangleMeshShape"); - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - m_data->m_collisionShapes.push_back(trimesh); - //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); - shape = trimesh; - if (compound) - { - compound->addChildShape(childTransform,shape); - } - } - delete glmesh; } else { + //ePrismaticType; @todo + btVector3 limitRange = linearLowerLimit.absolute()+linearUpperLimit.absolute(); + int limitAxis = limitRange.maxAxis(); - std::vector shapes; - std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + const btTransform& transA = con->getCalculatedTransformA(); + const btTransform& transB = con->getCalculatedTransformB(); + btVector3 axisA = transA.getBasis().getColumn(limitAxis); + btVector3 axisB = transB.getBasis().getColumn(limitAxis); - //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); - //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) - B3_PROFILE("createConvexHullFromShapes"); - if (compound==0) + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { - compound = worldImporter->createCompoundShape(); - } - compound->setMargin(defaultCollisionMargin); - - for (int s = 0; s<(int)shapes.size(); s++) - { - btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); - convexHull->setMargin(defaultCollisionMargin); - tinyobj::shape_t& shape = shapes[s]; - int faceCount = shape.mesh.indices.size(); - - for (int f = 0; faddPoint(pt*meshScale,false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); + con->getRigidBodyA().applyForce(-torque*axisA,btVector3(0,0,0)); + con->getRigidBodyB().applyForce(torque*axisB,btVector3(0,0,0)); + break; } - - convexHull->recalcLocalAabb(); - convexHull->optimizeConvexHull(); - compound->addChildShape(childTransform,convexHull); - } - } - } - } - break; - } - default: - { - } - } - if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN) - { - urdfCollisionObjects.push_back(urdfColObj); - } - } - - if (compound && compound->getNumChildShapes()) - { - shape = compound; - } - - if (shape) - { - int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); - InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); - handle->m_collisionShape = shape; - for (int i=0;im_urdfCollisionObjects.push_back(urdfCollisionObjects[i]); - } - serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid; - m_data->m_worldImporters.push_back(worldImporter); - serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; - } else - { - delete worldImporter; - } - - - break; - } - case CMD_CREATE_VISUAL_SHAPE: - { - hasStatus = true; - serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; - { - double globalScaling = 1.f; - - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); - u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); - btTransform localInertiaFrame; - localInertiaFrame.setIdentity(); - btTransform childTrans; - childTrans.setIdentity(); - const char* pathPrefix = ""; - if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1) - { - int userShapeIndex = 0; - - UrdfVisual visualShape; - visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type; - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - - if (visualShape.m_geometry.m_type == URDF_GEOM_MESH) - { - - std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName; - const std::string& error_message_prefix=""; - std::string out_found_filename; - int out_type; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) - { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } - - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); - visualShape.m_geometry.m_meshFileType = out_type; - visualShape.m_geometry.m_meshFileName=fileName; - - visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]); - } - visualShape.m_name = "bla"; - visualShape.m_materialName=""; - visualShape.m_sourceFileLocation="blaat_line_10"; - visualShape.m_linkLocalFrame.setIdentity(); - visualShape.m_geometry.m_hasLocalMaterial = false; - - - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - btAlignedObjectArray textures; - bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR)!=0;; - bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR)!=0;; - visualShape.m_geometry.m_hasLocalMaterial = hasRGBA|hasSpecular; - if (visualShape.m_geometry.m_hasLocalMaterial) - { - if (hasRGBA) - { - visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]); - } else - { - - } - if (hasSpecular) - { - visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]); - } else - { - visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4,0.4,0.4); - } - } - - if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform !=0) - { - childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[2])); - childTrans.setRotation(btQuaternion( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[2], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3])); - } - - - - u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); - - if (vertices.size() && indices.size()) - { - if (1) - { - int textureIndex = -1; - if (textures.size()) - { - - textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1,textures[0].m_width,textures[0].m_height); - } - int graphicsIndex = -1; - { - B3_PROFILE("registerGraphicsShape"); - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); - if (graphicsIndex>=0) - { - int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); - InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); - visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; - visualHandle->m_tinyRendererVisualShapeIndex = -1; - //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance - //store needed info for tinyrenderer - visualHandle->m_localInertiaFrame = localInertiaFrame; - visualHandle->m_visualShape = visualShape; - visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; - - serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; - serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; - } - } - } - } - } - } - - break; - } - case CMD_CREATE_MULTI_BODY: - { - hasStatus = true; - serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; - if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) - { - m_data->m_sdfRecentLoadedBodies.clear(); - - ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); - - bool useMultiBody = true; - if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) - { - useMultiBody = false; - } - - int flags = 0; - bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); - - if (ok) - { - int bodyUniqueId = -1; - - if (m_data->m_sdfRecentLoadedBodies.size()==1) - { - bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0]; - } - m_data->m_sdfRecentLoadedBodies.clear(); - if (bodyUniqueId>=0) - { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; - - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); - } - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - } - } - - //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); - - - } - break; - } - case CMD_SET_ADDITIONAL_SEARCH_PATH: - { - BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH"); - b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path); - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_LOAD_URDF: - { - BT_PROFILE("CMD_LOAD_URDF"); - const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); - } - btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); - btAssert(urdfArgs.m_urdfFileName); - btVector3 initialPos(0,0,0); - btQuaternion initialOrn(0,0,0,1); - if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) - { - initialPos[0] = urdfArgs.m_initialPosition[0]; - initialPos[1] = urdfArgs.m_initialPosition[1]; - initialPos[2] = urdfArgs.m_initialPosition[2]; - } - int urdfFlags = 0; - if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) - { - urdfFlags = urdfArgs.m_urdfFlags; - } - if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) - { - initialOrn[0] = urdfArgs.m_initialOrientation[0]; - initialOrn[1] = urdfArgs.m_initialOrientation[1]; - initialOrn[2] = urdfArgs.m_initialOrientation[2]; - initialOrn[3] = urdfArgs.m_initialOrientation[3]; - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true; - bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false; - int bodyUniqueId; - btScalar globalScaling = 1.f; - if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) - { - globalScaling = urdfArgs.m_globalScaling; - } - //load the actual URDF and send a report: completed or failed - bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, - initialPos,initialOrn, - useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling); - - if (completedOk && bodyUniqueId>=0) - { - - - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - - serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; - - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - - - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); - } - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - hasStatus = true; - - } else - { - serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; - hasStatus = true; - } - - - - - break; - } - case CMD_LOAD_BUNNY: - { - serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; - hasStatus = true; -#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - double scale = 0.1; - double mass = 0.1; - double collisionMargin = 0.02; - if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE) - { - scale = clientCmd.m_loadBunnyArguments.m_scale; - } - if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS) - { - mass = clientCmd.m_loadBunnyArguments.m_mass; - } - if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN) - { - collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin; - } - m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; - m_data->m_softBodyWorldInfo.water_density = 0; - m_data->m_softBodyWorldInfo.water_offset = 0; - m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); - m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); - m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; - m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); - - btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); - - btSoftBody::Material* pm=psb->appendMaterial(); - pm->m_kLST = 1.0; - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - psb->generateBendingConstraints(2,pm); - psb->m_cfg.piterations = 50; - psb->m_cfg.kDF = 0.5; - psb->randomizeConstraints(); - psb->rotate(btQuaternion(0.70711,0,0,0.70711)); - psb->translate(btVector3(0,0,1.0)); - psb->scale(btVector3(scale,scale,scale)); - psb->setTotalMass(mass,true); - psb->getCollisionShape()->setMargin(collisionMargin); - - m_data->m_dynamicsWorld->addSoftBody(psb); - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; -#endif - break; - } - case CMD_CREATE_SENSOR: - { - BT_PROFILE("CMD_CREATE_SENSOR"); - - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_CREATE_SENSOR"); - } - int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - btAssert(mb); - for (int i=0;igetLink(jointIndex).m_jointFeedback) - { - b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); - } else - { - btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); - fb->m_reactionForces.setZero(); - mb->getLink(jointIndex).m_jointFeedback = fb; - m_data->m_multiBodyJointFeedbacks.push_back(fb); - }; - - } else - { - if (mb->getLink(jointIndex).m_jointFeedback) - { - m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); - delete mb->getLink(jointIndex).m_jointFeedback; - mb->getLink(jointIndex).m_jointFeedback=0; - } else - { - b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); - }; - - } - } - - } else - { - b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); - } - -#if 0 - //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody - /* - for (int i=0;im_dynamicsWorld->getNumConstraints();i++) - { - btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); - btJointFeedback* fb = new btJointFeedback(); - m_data->m_jointFeedbacks.push_back(fb); - c->setJointFeedback(fb); - - - } - */ -#endif - - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - - break; - } - case CMD_PROFILE_TIMING: - { - { - B3_PROFILE("custom");//clientCmd.m_profile.m_name); - { - B3_PROFILE("event");//clientCmd.m_profile.m_name); - char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name]; - char* eventName = 0; - if (eventNamePtr) - { - B3_PROFILE("reuse"); - eventName = *eventNamePtr; - - } else - { - B3_PROFILE("alloc"); - int len = strlen(clientCmd.m_profile.m_name); - eventName = new char[len+1]; - strcpy(eventName,clientCmd.m_profile.m_name); - eventName[len] = 0; - m_data->m_profileEvents.insert(eventName,eventName); - - } - - - { - { - B3_PROFILE("with");//clientCmd.m_profile.m_name); - { - B3_PROFILE("some");//clientCmd.m_profile.m_name); - { - B3_PROFILE("deep");//clientCmd.m_profile.m_name); - { - B3_PROFILE("level");//clientCmd.m_profile.m_name); + case CONTROL_MODE_VELOCITY: { - B3_PROFILE(eventName); - b3Clock::usleep(clientCmd.m_profile.m_durationInMicroSeconds); + con->enableMotor(limitAxis,true); + con->setTargetVelocity(limitAxis, -qdotTarget); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(limitAxis,torqueImpulse); + break; } - } - } - } - } - } - } - } - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - - case CMD_SEND_DESIRED_STATE: - { - BT_PROFILE("CMD_SEND_DESIRED_STATE"); - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_SEND_DESIRED_STATE"); - } - - int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - btAssert(mb); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_TORQUE"); - } - // mb->clearForcesAndTorques(); - int torqueIndex = 6; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - for (int link=0;linkgetNumLinks();link++) - { - - for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - double torque = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - mb->addJointTorqueMultiDof(link,dof,torque); - } - torqueIndex++; - } - } - } - break; - } - case CONTROL_MODE_VELOCITY: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_VELOCITY"); - } - - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) + case CONTROL_MODE_POSITION_VELOCITY_PD: { - - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - - if (motor) - { - btScalar desiredVelocity = 0.f; - bool hasDesiredVelocity = false; - - - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) - { - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - btScalar kd = 0.1f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) - { - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; - } - - motor->setVelocityTarget(desiredVelocity,kd); - - btScalar kp = 0.f; - motor->setPositionTarget(0,kp); - hasDesiredVelocity = true; - } - if (hasDesiredVelocity) - { - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; - } - motor->setMaxAppliedImpulse(maxImp); - } - numMotors++; - - } + con->setServo(limitAxis,true); + con->setServoTarget(limitAxis,qTarget); + //next one is the maximum velocity to reach target position. + //the maximum velocity is limited by maxMotorForce + con->setTargetVelocity(limitAxis, 100); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(limitAxis,torqueImpulse); + con->enableMotor(limitAxis,true); + break; } - dofIndex += mb->getLink(link).m_dofCount; - } - } - break; - } - - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); - } - //compute the force base on PD control - - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base - int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) + default: { - - - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - if (motor) - { - - bool hasDesiredPosOrVel = false; - btScalar kp = 0.f; - btScalar kd = 0.f; - btScalar desiredVelocity = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) - { - hasDesiredPosOrVel = true; - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - kd = 0.1; - } - btScalar desiredPosition = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) - { - hasDesiredPosOrVel = true; - desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - kp = 0.1; - } - - if (hasDesiredPosOrVel) - { - - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) - { - kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - } - - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) - { - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; - } - - motor->setVelocityTarget(desiredVelocity,kd); - motor->setPositionTarget(desiredPosition,kp); - - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; - - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(maxImp); - } - numMotors++; - } - } - velIndex += mb->getLink(link).m_dofCount; - posIndex += mb->getLink(link).m_posVarCount; - } - } + }; - break; + } } - default: - { - b3Warning("m_controlMode not implemented yet"); - break; - } + }//fi + ///see addJointInfoFromConstraint + velIndex ++;//info.m_uIndex + posIndex ++;//info.m_qIndex - } - } else - { - //support for non-btMultiBody, such as btRigidBody - - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - btAssert(rb); - - //switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - //case CONTROL_MODE_TORQUE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_TORQUE"); - } - // mb->clearForcesAndTorques(); - ///see addJointInfoFromConstraint - int velIndex = 6; - int posIndex = 7; - //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - for (int link=0;linkm_rigidBodyJoints.size();link++) - { - btGeneric6DofSpring2Constraint* con = body->m_rigidBodyJoints[link]; - - btVector3 linearLowerLimit; - btVector3 linearUpperLimit; - btVector3 angularLowerLimit; - btVector3 angularUpperLimit; - - - //for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - - - { - - int torqueIndex = velIndex; - double torque = 100; - bool hasDesiredTorque = false; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]; - hasDesiredTorque = true; - } - - bool hasDesiredPosOrVel = false; - btScalar qdotTarget = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) - { - hasDesiredPosOrVel = true; - qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - } - btScalar qTarget = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) - { - hasDesiredPosOrVel = true; - qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - } - - con->getLinearLowerLimit(linearLowerLimit); - con->getLinearUpperLimit(linearUpperLimit); - con->getAngularLowerLimit(angularLowerLimit); - con->getAngularUpperLimit(angularUpperLimit); - - if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero()) - { - //fixed, don't do anything - } else - { - con->calculateTransforms(); - - if (linearLowerLimit.isZero() && linearUpperLimit.isZero()) - { - //eRevoluteType; - btVector3 limitRange = angularLowerLimit.absolute()+angularUpperLimit.absolute(); - int limitAxis = limitRange.maxAxis(); - const btTransform& transA = con->getCalculatedTransformA(); - const btTransform& transB = con->getCalculatedTransformB(); - btVector3 axisA = transA.getBasis().getColumn(limitAxis); - btVector3 axisB = transB.getBasis().getColumn(limitAxis); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - if (hasDesiredTorque) - { - con->getRigidBodyA().applyTorque(torque*axisA); - con->getRigidBodyB().applyTorque(-torque*axisB); - } - break; - } - case CONTROL_MODE_VELOCITY: - { - if (hasDesiredPosOrVel) - { - con->enableMotor(3+limitAxis,true); - con->setTargetVelocity(3+limitAxis, qdotTarget); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); - } - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - if (hasDesiredPosOrVel) - { - con->setServo(3+limitAxis,true); - con->setServoTarget(3+limitAxis,-qTarget); - //next one is the maximum velocity to reach target position. - //the maximum velocity is limited by maxMotorForce - con->setTargetVelocity(3+limitAxis, 100); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); - con->enableMotor(3+limitAxis,true); - } - break; - } - default: - { - } - }; - - - - - } else - { - //ePrismaticType; @todo - btVector3 limitRange = linearLowerLimit.absolute()+linearUpperLimit.absolute(); - int limitAxis = limitRange.maxAxis(); - - const btTransform& transA = con->getCalculatedTransformA(); - const btTransform& transB = con->getCalculatedTransformB(); - btVector3 axisA = transA.getBasis().getColumn(limitAxis); - btVector3 axisB = transB.getBasis().getColumn(limitAxis); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - con->getRigidBodyA().applyForce(-torque*axisA,btVector3(0,0,0)); - con->getRigidBodyB().applyForce(torque*axisB,btVector3(0,0,0)); - break; - } - case CONTROL_MODE_VELOCITY: - { - con->enableMotor(limitAxis,true); - con->setTargetVelocity(limitAxis, -qdotTarget); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(limitAxis,torqueImpulse); - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - con->setServo(limitAxis,true); - con->setServoTarget(limitAxis,qTarget); - //next one is the maximum velocity to reach target position. - //the maximum velocity is limited by maxMotorForce - con->setTargetVelocity(limitAxis, 100); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(limitAxis,torqueImpulse); - con->enableMotor(limitAxis,true); - break; - } - default: - { - } - }; - - } - } - }//fi - ///see addJointInfoFromConstraint - velIndex ++;//info.m_uIndex - posIndex ++;//info.m_qIndex - - } - } - }//fi - //break; - } - - } - } //if (body && body->m_rigidBody) - } - - serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; - hasStatus = true; - break; - } - case CMD_REQUEST_COLLISION_INFO: - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; - hasStatus=true; - int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; - serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; - - if (body->m_multiBody->getBaseCollider()) - { - btTransform tr; - tr.setOrigin(mb->getBasePos()); - tr.setRotation(mb->getWorldToBaseRot().inverse()); - - btVector3 aabbMin,aabbMax; - body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; - } - for (int l=0;lgetNumLinks();l++) - { - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = 0; - - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = -1; - - - - if (body->m_multiBody->getLink(l).m_collider) - { - btVector3 aabbMin,aabbMax; - body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform,aabbMin,aabbMax); - - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2]; - } - - } - } - else - { - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; - serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; - if (rb->getCollisionShape()) - { - btTransform tr = rb->getWorldTransform(); - - btVector3 aabbMin,aabbMax; - rb->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; } } - } - break; + }//fi + //break; } - case CMD_REQUEST_ACTUAL_STATE: - { - BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); - if (m_data->m_verboseOutput) - { - b3Printf("Sending the actual state (Q,U)"); - } - int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks(); - - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) - { - serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; - hasStatus = true; - break; - } - - //always add the base, even for static (non-moving objects) - //so that we can easily move the 'fixed' base when needed - //do we don't use this conditional "if (!mb->hasFixedBase())" - { - btTransform tr; - tr.setOrigin(mb->getBasePos()); - tr.setRotation(mb->getWorldToBaseRot().inverse()); - - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = - body->m_rootLocalInertialFrame.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = - body->m_rootLocalInertialFrame.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = - body->m_rootLocalInertialFrame.getOrigin()[2]; - - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = - body->m_rootLocalInertialFrame.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = - body->m_rootLocalInertialFrame.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = - body->m_rootLocalInertialFrame.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = - body->m_rootLocalInertialFrame.getRotation()[3]; - - - - //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - - //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ +=7;//pos + quaternion - - //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; - - //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; - totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF - } - - btAlignedObjectArray omega; - btAlignedObjectArray linVel; - - bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); - if (computeForwardKinematics) - { - B3_PROFILE("compForwardKinematics"); - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; - world_to_local.resize(mb->getNumLinks()+1); - local_origin.resize(mb->getNumLinks()+1); - mb->forwardKinematics(world_to_local,local_origin); - } - - bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); - if (computeLinkVelocities) - { - omega.resize(mb->getNumLinks()+1); - linVel.resize(mb->getNumLinks()+1); - { - B3_PROFILE("compTreeLinkVelocities"); - mb->compTreeLinkVelocities(&omega[0], &linVel[0]); - } - } - for (int l=0;lgetNumLinks();l++) - { - for (int d=0;dgetLink(l).m_posVarCount;d++) - { - serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; - } - for (int d=0;dgetLink(l).m_dofCount;d++) - { - serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; - } - - if (0 == mb->getLink(l).m_jointFeedback) - { - for (int d=0;d<6;d++) - { - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; - } - } else - { - btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); - btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; - - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; - } - - serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; - - if (supportsJointMotor(mb,l)) - { - - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; - - if (motor && m_data->m_physicsDeltaTime>btScalar(0)) - { - btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; - serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = - force; - //if (force>0) - //{ - // b3Printf("force = %f\n", force); - //} - } - } - btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); - btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); - - btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); - btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); - - serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w(); - - - - btVector3 worldLinVel(0,0,0); - btVector3 worldAngVel(0,0,0); - - if (computeLinkVelocities) - { - const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); - worldLinVel = linkRotMat * linVel[l+1]; - worldAngVel = linkRotMat * omega[l+1]; - } - - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = worldLinVel[0]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = worldLinVel[1]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = worldLinVel[2]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = worldAngVel[0]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = worldAngVel[1]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = worldAngVel[2]; - - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); - - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); - - } - - - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - - hasStatus = true; - - } else - { - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - serverCmd.m_sendActualStateArgs.m_numLinks = 0; - - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - btTransform tr = rb->getWorldTransform(); - //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - - //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ +=7;//pos + quaternion - - //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - - //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; - totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF - - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - - hasStatus = true; - } else - { - //b3Warning("Request state but no multibody or rigid body available"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; - hasStatus = true; - } - } - - break; - } - case CMD_STEP_FORWARD_SIMULATION: - { - BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); - - - if (m_data->m_verboseOutput) - { - b3Printf("Step simulation request"); - b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); - } - ///todo(erwincoumans) move this damping inside Bullet - for (int i=0;im_dynamicsWorld->getNumMultibodies();i++) - { - btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); - for (int l=0;lgetNumLinks();l++) - { - for (int d=0;dgetLink(l).m_dofCount;d++) - { - double damping_coefficient = mb->getLink(l).m_jointDamping; - double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d]; - mb->addJointTorqueMultiDof(l, d, damping); - } - } - } - - btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor; - - if (m_data->m_numSimulationSubSteps > 0) - { - m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); - } - else - { - m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0); - } - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; - hasStatus = true; - break; - } - - case CMD_REQUEST_INTERNAL_DATA: - { - BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); - - //todo: also check version etc? - - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_FAILED; - hasStatus = true; - - int sz = btDefaultSerializer::getMemoryDnaSizeInBytes(); - const char* memDna = btDefaultSerializer::getMemoryDna(); - if (sz < bufferSizeInBytes) - { - for (int i = 0; i < sz; i++) - { - bufferServerToClient[i] = memDna[i]; - } - serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; - serverCmd.m_numDataStreamBytes = sz; - } - - break; - }; - case CMD_CHANGE_DYNAMICS_INFO: - { - BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO"); - - int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; - double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; - double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; - double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction; - double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction; - double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution; - btAssert(bodyUniqueId >= 0); - - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } - - if (linkIndex == -1) - { - if (mb->getBaseCollider()) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - mb->getBaseCollider()->setRestitution(restitution); - } - - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - mb->getBaseCollider()->setFriction(lateralFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - mb->getBaseCollider()->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - mb->getBaseCollider()->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } else - { - mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - } - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - mb->setBaseMass(mass); - if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) - { - btVector3 localInertia; - mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia); - mb->setBaseInertia(localInertia); - } - } - } - else - { - if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) - { - if (mb->getLinkCollider(linkIndex)) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - mb->getLinkCollider(linkIndex)->setRestitution(restitution); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } else - { - mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - } - - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - - - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - mb->getLink(linkIndex).m_mass = mass; - if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) - { - btVector3 localInertia; - mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia); - mb->getLink(linkIndex).m_inertiaLocal = localInertia; - } - } - } - } - } else - { - if (body && body->m_rigidBody) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - btScalar angDamping = body->m_rigidBody->getAngularDamping(); - body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - btScalar linDamping = body->m_rigidBody->getLinearDamping(); - body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - body->m_rigidBody->setRestitution(restitution); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - body->m_rigidBody->setFriction(lateralFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - body->m_rigidBody->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - body->m_rigidBody->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } else - { - body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - btVector3 localInertia; - if (body->m_rigidBody->getCollisionShape()) - { - body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia); - } - body->m_rigidBody->setMassProps(mass,localInertia); - } - } - } - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - - break; - }; - case CMD_GET_DYNAMICS_INFO: - { - int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body && body->m_multiBody) - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - - btMultiBody* mb = body->m_multiBody; - if (linkIndex == -1) - { - serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass(); - serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); - } - else - { - serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex); - if (mb->getLinkCollider(linkIndex)) - { - serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); - } - else - { - b3Warning("The dynamic info requested is not available"); - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; - } - } - hasStatus = true; - } - else - { - b3Warning("The dynamic info requested is not available"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; - hasStatus = true; - } - break; - } - - case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: - { - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED; - serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; - serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; - serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2; - serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp; - serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; - serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled(); - serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; - btVector3 grav = m_data->m_dynamicsWorld->getGravity(); - serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0]; - serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1]; - serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; - serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; - serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps; - serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; - serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; - serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; - serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation; - serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; - hasStatus = true; - break; - } - - case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: - { - BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) - { - m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; - } - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) - { - m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); - } - - //see - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) - { - //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk - gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) - { - btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], - clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], - clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); - this->m_data->m_dynamicsWorld->setGravity(grav); - if (m_data->m_verboseOutput) - { - b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); - } - - } - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) - { - m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; - } - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) - { - gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) - { - m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; - } - - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) - { - m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; - } - if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) - { - m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; - } - - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) - { - m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) - { - m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) - { - m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) - { - m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; - } - - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) - { - m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; - } - - - - if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) - { - b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); - } - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - - }; - case CMD_INIT_POSE: - { - BT_PROFILE("CMD_INIT_POSE"); - - if (m_data->m_verboseOutput) - { - b3Printf("Server Init Pose not implemented yet"); - } - int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - btVector3 baseLinVel(0, 0, 0); - btVector3 baseAngVel(0, 0, 0); - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], - clientCmd.m_initPoseArgs.m_initialStateQdot[1], - clientCmd.m_initPoseArgs.m_initialStateQdot[2]); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], - clientCmd.m_initPoseArgs.m_initialStateQdot[4], - clientCmd.m_initPoseArgs.m_initialStateQdot[5]); - } - btVector3 basePos(0, 0, 0); - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - basePos = btVector3( - clientCmd.m_initPoseArgs.m_initialStateQ[0], - clientCmd.m_initPoseArgs.m_initialStateQ[1], - clientCmd.m_initPoseArgs.m_initialStateQ[2]); - } - btQuaternion baseOrn(0, 0, 0, 1); - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3], - clientCmd.m_initPoseArgs.m_initialStateQ[4], - clientCmd.m_initPoseArgs.m_initialStateQ[5], - clientCmd.m_initPoseArgs.m_initialStateQ[6]); - } - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - - - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - mb->setBaseVel(baseLinVel); - } - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - mb->setBaseOmega(baseAngVel); - } - - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - btVector3 zero(0,0,0); - btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); - - mb->setBaseVel(baseLinVel); - mb->setBasePos(basePos); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); - - mb->setBaseOmega(baseAngVel); - btQuaternion invOrn(baseOrn); - - mb->setWorldToBaseRot(invOrn.inverse()); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) - { - int uDofIndex = 6; - int posVarCountIndex = 7; - for (int i=0;igetNumLinks();i++) - { - if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount==1)) - { - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]); - mb->setJointVel(i,0);//backwards compatibility - } - if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount==1)) - { - btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]; - mb->setJointVel(i,vel); - } - - posVarCountIndex += mb->getLink(i).m_posVarCount; - uDofIndex += mb->getLink(i).m_dofCount; - - } - } - - btAlignedObjectArray scratch_q; - btAlignedObjectArray scratch_m; - - mb->forwardKinematics(scratch_q,scratch_m); - int nLinks = mb->getNumLinks(); - scratch_q.resize(nLinks+1); - scratch_m.resize(nLinks+1); - - mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m); - - - } - - if (body && body->m_rigidBody) - { - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - body->m_rigidBody->setLinearVelocity(baseLinVel); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - body->m_rigidBody->setAngularVelocity(baseAngVel); - } - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - body->m_rigidBody->getWorldTransform().setOrigin(basePos); - body->m_rigidBody->setLinearVelocity(baseLinVel); - } - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - body->m_rigidBody->getWorldTransform().setRotation(baseOrn); - body->m_rigidBody->setAngularVelocity(baseAngVel); - } - - } - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - - break; - } - - - case CMD_RESET_SIMULATION: - { - - BT_PROFILE("CMD_RESET_SIMULATION"); - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); - resetSimulation(); - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; - hasStatus = true; - break; - } - case CMD_CREATE_RIGID_BODY: - case CMD_CREATE_BOX_COLLISION_SHAPE: - { - BT_PROFILE("CMD_CREATE_RIGID_BODY"); - - btVector3 halfExtents(1,1,1); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) - { - halfExtents = btVector3( - clientCmd.m_createBoxShapeArguments.m_halfExtentsX, - clientCmd.m_createBoxShapeArguments.m_halfExtentsY, - clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); - } - btTransform startTrans; - startTrans.setIdentity(); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) - { - startTrans.setOrigin(btVector3( - clientCmd.m_createBoxShapeArguments.m_initialPosition[0], - clientCmd.m_createBoxShapeArguments.m_initialPosition[1], - clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); - } - - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) - { - - startTrans.setRotation(btQuaternion( - clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); - } - - btScalar mass = 0.f; - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) - { - mass = clientCmd.m_createBoxShapeArguments.m_mass; - } - - int shapeType = COLLISION_SHAPE_TYPE_BOX; - - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) - { - shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; - } - - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); - m_data->m_worldImporters.push_back(worldImporter); - - btCollisionShape* shape = 0; - - switch (shapeType) - { - case COLLISION_SHAPE_TYPE_CYLINDER_X: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[0]; - shape = worldImporter->createCylinderShapeX(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CYLINDER_Y: - { - btScalar radius = halfExtents[0]; - btScalar height = halfExtents[1]; - shape = worldImporter->createCylinderShapeY(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CYLINDER_Z: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[2]; - shape = worldImporter->createCylinderShapeZ(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_X: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[0]; - shape = worldImporter->createCapsuleShapeX(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_Y: - { - btScalar radius = halfExtents[0]; - btScalar height = halfExtents[1]; - shape = worldImporter->createCapsuleShapeY(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_Z: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[2]; - shape = worldImporter->createCapsuleShapeZ(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_SPHERE: - { - btScalar radius = halfExtents[0]; - shape = worldImporter->createSphereShape(radius); - break; - } - case COLLISION_SHAPE_TYPE_BOX: - default: - { - shape = worldImporter->createBoxShape(halfExtents); - } - } - - - bool isDynamic = (mass>0); - btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); - //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - btVector4 colorRGBA(1,0,0,1); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) - { - colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; - colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; - colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; - colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; - } - m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); - m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; - - - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; - rb->setUserIndex2(bodyUniqueId); - bodyHandle->m_rootLocalInertialFrame.setIdentity(); - bodyHandle->m_rigidBody = rb; - hasStatus = true; - break; - } - case CMD_PICK_BODY: - { - BT_PROFILE("CMD_PICK_BODY"); - - pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], - clientCmd.m_pickBodyArguments.m_rayFromWorld[1], - clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), - btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], - clientCmd.m_pickBodyArguments.m_rayToWorld[1], - clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_MOVE_PICKED_BODY: - { - BT_PROFILE("CMD_MOVE_PICKED_BODY"); - - movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], - clientCmd.m_pickBodyArguments.m_rayFromWorld[1], - clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), - btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], - clientCmd.m_pickBodyArguments.m_rayToWorld[1], - clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_REMOVE_PICKING_CONSTRAINT_BODY: - { - BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY"); - removePickingConstraint(); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_REQUEST_AABB_OVERLAP: - { - BT_PROFILE("CMD_REQUEST_AABB_OVERLAP"); - SharedMemoryStatus& serverCmd = serverStatusOut; - int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; - - if (0== curObjectIndex) - { - //clientCmd.m_requestContactPointArguments.m_aabbQueryMin - btVector3 aabbMin, aabbMax; - aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]); - aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]); - - m_data->m_cachedOverlappingObjects.clear(); - - m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects); - } - - - int totalBytesPerObject = sizeof(b3OverlappingObject); - int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1; - int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); - int remainingObjects = numOverlap - curObjectIndex; - - int curNumObjects = btMin(overlapCapacity, remainingObjects); - - if (numOverlap < overlapCapacity) - { - - b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient; - for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++) - { - overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i]; - overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i]; - } - - serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED; - - //int m_startingOverlappingObjectIndex; - //int m_numOverlappingObjectsCopied; - //int m_numRemainingOverlappingObjects; - serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; - serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); - serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects; - } - else - { - serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED; - } - - hasStatus = true; - break; - } - - case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: - { - BT_PROFILE("CMD_REQUEST_OPENGL_VISUALIZER_CAMERA"); - SharedMemoryStatus& serverCmd = serverStatusOut; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &serverCmd.m_visualizerCameraResultArgs.m_width, - &serverCmd.m_visualizerCameraResultArgs.m_height, - serverCmd.m_visualizerCameraResultArgs.m_viewMatrix, - serverCmd.m_visualizerCameraResultArgs.m_projectionMatrix, - serverCmd.m_visualizerCameraResultArgs.m_camUp, - serverCmd.m_visualizerCameraResultArgs.m_camForward, - serverCmd.m_visualizerCameraResultArgs.m_horizontal, - serverCmd.m_visualizerCameraResultArgs.m_vertical, - &serverCmd.m_visualizerCameraResultArgs.m_yaw, - &serverCmd.m_visualizerCameraResultArgs.m_pitch, - &serverCmd.m_visualizerCameraResultArgs.m_dist, - serverCmd.m_visualizerCameraResultArgs.m_target); - serverCmd.m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED; - hasStatus = true; - break; - } - - case CMD_CONFIGURE_OPENGL_VISUALIZER: - { - BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type =CMD_CLIENT_COMMAND_COMPLETED; - - hasStatus = true; - - if (clientCmd.m_updateFlags&COV_SET_FLAGS) - { - if (clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag == COV_ENABLE_TINY_RENDERER) - { - m_data->m_enableTinyRenderer = clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled!=0; - } - m_data->m_guiHelper->setVisualizerFlag(clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag, - clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled); - } - if (clientCmd.m_updateFlags&COV_SET_CAMERA_VIEW_MATRIX) - { - m_data->m_guiHelper->resetCamera( clientCmd.m_configureOpenGLVisualizerArguments.m_cameraDistance, - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraYaw, - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraPitch, - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0], - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1], - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]); - } - break; - } - - case CMD_REQUEST_CONTACT_POINT_INFORMATION: - { - BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION"); - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; - - //make a snapshot of the contact manifolds into individual contact points - if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0) - { - m_data->m_cachedContactPoints.resize(0); - - int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS; - - if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE) - { - mode = clientCmd.m_requestContactPointArguments.m_mode; - } - - switch (mode) - { - case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS: - { - int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds(); - m_data->m_cachedContactPoints.reserve(numContactManifolds * 4); - for (int i = 0; i < numContactManifolds; i++) - { - const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; - int linkIndexA = -1; - int linkIndexB = -1; - - int objectIndexB = -1; - const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); - if (bodyB) - { - objectIndexB = bodyB->getUserIndex2(); - } - const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - if (mblB && mblB->m_multiBody) - { - linkIndexB = mblB->m_link; - objectIndexB = mblB->m_multiBody->getUserIndex2(); - } - - int objectIndexA = -1; - const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0()); - if (bodyA) - { - objectIndexA = bodyA->getUserIndex2(); - } - const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - if (mblA && mblA->m_multiBody) - { - linkIndexA = mblA->m_link; - objectIndexA = mblA->m_multiBody->getUserIndex2(); - } - btAssert(bodyA || mblA); - - //apply the filter, if the user provides it - bool swap = false; - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) - { - swap = false; - } - else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) - { - swap = true; - } - else - { - continue; - } - } - - if (swap) - { - std::swap(objectIndexA, objectIndexB); - std::swap(linkIndexA, linkIndexB); - std::swap(bodyA, bodyB); - } - - //apply the second object filter, if the user provides it - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) - { - continue; - } - } - - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) - { - continue; - } - - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) - { - continue; - } - - for (int p = 0; p < manifold->getNumContacts(); p++) - { - - b3ContactPointData pt; - pt.m_bodyUniqueIdA = objectIndexA; - pt.m_bodyUniqueIdB = objectIndexB; - const btManifoldPoint& srcPt = manifold->getContactPoint(p); - pt.m_contactDistance = srcPt.getDistance(); - pt.m_contactFlags = 0; - pt.m_linkIndexA = linkIndexA; - pt.m_linkIndexB = linkIndexB; - for (int j = 0; j < 3; j++) - { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; - } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; - m_data->m_cachedContactPoints.push_back(pt); - } - } - break; - } - - case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS: - { - //todo(erwincoumans) compute closest points between all, and vs all, pair - btScalar closestDistanceThreshold = 0.f; - - if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD) - { - closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold; - } - - int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter; - int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter; - - bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER)); - bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER)); - - int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter; - int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter; - - btAlignedObjectArray setA; - btAlignedObjectArray setB; - btAlignedObjectArray setALinkIndex; - btAlignedObjectArray setBLinkIndex; - - if (bodyUniqueIdA >= 0) - { - InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); - if (bodyA) - { - if (bodyA->m_multiBody) - { - if (bodyA->m_multiBody->getBaseCollider()) - { - if (!hasLinkIndexAFilter || (linkIndexA == -1)) - { - setA.push_back(bodyA->m_multiBody->getBaseCollider()); - setALinkIndex.push_back(-1); - } - } - for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++) - { - if (bodyA->m_multiBody->getLink(i).m_collider) - { - if (!hasLinkIndexAFilter || (linkIndexA == i)) - { - setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); - setALinkIndex.push_back(i); - } - } - } - } - if (bodyA->m_rigidBody) - { - setA.push_back(bodyA->m_rigidBody); - setALinkIndex.push_back(-1); - } - } - } - if (bodyUniqueIdB>=0) - { - InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); - if (bodyB) - { - if (bodyB->m_multiBody) - { - if (bodyB->m_multiBody->getBaseCollider()) - { - if (!hasLinkIndexBFilter || (linkIndexB == -1)) - { - setB.push_back(bodyB->m_multiBody->getBaseCollider()); - setBLinkIndex.push_back(-1); - } - } - for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++) - { - if (bodyB->m_multiBody->getLink(i).m_collider) - { - if (!hasLinkIndexBFilter || (linkIndexB ==i)) - { - setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); - setBLinkIndex.push_back(i); - } - } - } - } - if (bodyB->m_rigidBody) - { - setB.push_back(bodyB->m_rigidBody); - setBLinkIndex.push_back(-1); - - } - } - } - - { - ///ContactResultCallback is used to report contact points - struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback - { - int m_bodyUniqueIdA; - int m_bodyUniqueIdB; - int m_linkIndexA; - int m_linkIndexB; - btScalar m_deltaTime; - - btAlignedObjectArray& m_cachedContactPoints; - - MyContactResultCallback(btAlignedObjectArray& pointCache) - :m_cachedContactPoints(pointCache) - { - } - - virtual ~MyContactResultCallback() - { - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - //bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; - //collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); - //return collides; - return true; - } - - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) - { - if (cp.m_distance1<=m_closestDistanceThreshold) - { - b3ContactPointData pt; - pt.m_bodyUniqueIdA = m_bodyUniqueIdA; - pt.m_bodyUniqueIdB = m_bodyUniqueIdB; - const btManifoldPoint& srcPt = cp; - pt.m_contactDistance = srcPt.getDistance(); - pt.m_contactFlags = 0; - pt.m_linkIndexA = m_linkIndexA; - pt.m_linkIndexB = m_linkIndexB; - for (int j = 0; j < 3; j++) - { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; - } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; - m_cachedContactPoints.push_back(pt); - } - return 1; - - } - }; - - - MyContactResultCallback cb(m_data->m_cachedContactPoints); - - cb.m_bodyUniqueIdA = bodyUniqueIdA; - cb.m_bodyUniqueIdB = bodyUniqueIdB; - cb.m_deltaTime = m_data->m_physicsDeltaTime; - - for (int i = 0; i < setA.size(); i++) - { - cb.m_linkIndexA = setALinkIndex[i]; - for (int j = 0; j < setB.size(); j++) - { - cb.m_linkIndexB = setBLinkIndex[j]; - cb.m_closestDistanceThreshold = closestDistanceThreshold; - this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb); - } - } - } - - break; - } - default: - { - b3Warning("Unknown contact query mode: %d", mode); - } - - } - } - - int numContactPoints = m_data->m_cachedContactPoints.size(); - - - //b3ContactPoint - //struct b3ContactPointDynamics - - int totalBytesPerContact = sizeof(b3ContactPointData); - int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1; - - b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; - - int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; - int numContactPointBatch = btMin(numContactPoints,contactPointStorage); - - int endContactPointIndex = startContactPointIndex+numContactPointBatch; - - for (int i=startContactPointIndex;im_cachedContactPoints[i]; - b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; - destPt = srcPt; - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; - } - - serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; - serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; - serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; - serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED, - hasStatus = true; - break; - } - case CMD_CALCULATE_INVERSE_DYNAMICS: - { - BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS"); - SharedMemoryStatus& serverCmd = serverStatusOut; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); - for (int i = 0; i < num_dofs; i++) - { - q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; - qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; - nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; - for (int i = 0; i < num_dofs; i++) - { - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs]; - } - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; - } - } - - } - else - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; - } - - hasStatus = true; - break; - } - case CMD_CALCULATE_JACOBIAN: - { - BT_PROFILE("CMD_CALCULATE_JACOBIAN"); - - SharedMemoryStatus& serverCmd = serverStatusOut; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx q(numDofs + baseDofs); - btInverseDynamics::vecx qdot(numDofs + baseDofs); - btInverseDynamics::vecx nu(numDofs + baseDofs); - btInverseDynamics::vecx joint_force(numDofs + baseDofs); - for (int i = 0; i < numDofs; i++) - { - q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i]; - qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i]; - nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs; - // Set jacobian value - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); - btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs); - - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. - tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t); - tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r); - // Update the translational jacobian based on the desired local point. - // v_pt = v_frame + w x pt - // v_pt = J_t * qd + (J_r * qd) x pt - // v_pt = J_t * qd - pt x (J_r * qd) - // v_pt = J_t * qd - pt_x * J_r * qd) - // v_pt = (J_t - pt_x * J_r) * qd - // J_t_new = J_t - pt_x * J_r - btInverseDynamics::vec3 localPosition; - for (int i = 0; i < 3; ++i) { - localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i]; - } - // Only calculate if the localPosition is non-zero. - if (btInverseDynamics::maxAbs(localPosition) > 0.0) { - // Write the localPosition into world coordinates. - btInverseDynamics::mat33 world_rotation_body; - tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body); - localPosition = world_rotation_body * localPosition; - // Correct the translational jacobian. - btInverseDynamics::mat33 skewCrossProduct; - btInverseDynamics::skew(localPosition, &skewCrossProduct); - btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs); - btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l); - btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs); - btInverseDynamics::sub(jac_t, jac_l, &jac_t_new); - jac_t = jac_t_new; - } - // Fill in the result into the shared memory. - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < (numDofs + baseDofs); ++j) - { - int element = (numDofs + baseDofs) * i + j; - serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j); - serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j); - } - } - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; - } - } - - } - else - { - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; - } - - hasStatus = true; - break; - } - case CMD_CALCULATE_MASS_MATRIX: - { - BT_PROFILE("CMD_CALCULATE_MASS_MATRIX"); - - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - const int totDofs = numDofs + baseDofs; - btInverseDynamics::vecx q(totDofs); - btInverseDynamics::matxx massMatrix(totDofs, totDofs); - for (int i = 0; i < numDofs; i++) - { - q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i]; - } - if (-1 != tree->calculateMassMatrix(q, &massMatrix)) - { - serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; - // Fill in the result into the shared memory. - double* sharedBuf = (double*)bufferServerToClient; - int sizeInBytes = totDofs*totDofs*sizeof(double); - if (sizeInBytes < bufferSizeInBytes) - { - for (int i = 0; i < (totDofs); ++i) - { - for (int j = 0; j < (totDofs); ++j) - { - int element = (totDofs) * i + j; - - sharedBuf[element] = massMatrix(i,j); - } - } - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; - } - } - - } - - } - else - { - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; - } - - hasStatus = true; - break; - } - case CMD_APPLY_EXTERNAL_FORCE: - { - BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE"); - - if (m_data->m_verboseOutput) - { - b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber); - } - for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) - { - InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); - bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) - { - btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - btVector3 tmpPosition( - clientCmd.m_externalForceArguments.m_positions[i*3+0], - clientCmd.m_externalForceArguments.m_positions[i*3+1], - clientCmd.m_externalForceArguments.m_positions[i*3+2]); - - - if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) - { - btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpForce : tmpForce; - btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); - mb->addBaseForce(forceWorld); - mb->addBaseTorque(relPosWorld.cross(forceWorld)); - //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); - } else - { - int link = clientCmd.m_externalForceArguments.m_linkIds[i]; - - btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpForce : tmpForce; - btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); - mb->addLinkForce(link, forceWorld); - mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); - //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); - } - } - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) - { - btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - - if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) - { - btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; - mb->addBaseTorque(torqueWorld); - //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); - } else - { - int link = clientCmd.m_externalForceArguments.m_linkIds[i]; - btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; - mb->addLinkTorque(link, torqueWorld); - //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); - } - } - } - - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) - { - btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); - btVector3 positionLocal( - clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], - clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], - clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); - - btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal; - btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal; - rb->applyForce(forceWorld, relPosWorld); - - } - - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0) - { - btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); - - btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal; - rb->applyTorque(torqueWorld); - } - } - } - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_REMOVE_BODY: - { - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_REMOVE_BODY_FAILED; - serverCmd.m_removeObjectArgs.m_numBodies = 0; - serverCmd.m_removeObjectArgs.m_numUserConstraints = 0; - - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); - - for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); - if (bodyHandle) - { - if (bodyHandle->m_multiBody) - { - serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; - - //also remove user constraints... - for (int i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--) - { - btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); - if ((mbc->getMultiBodyA() == bodyHandle->m_multiBody)||(mbc->getMultiBodyB()==bodyHandle->m_multiBody)) - { - m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbc); - - //also remove user constraint and submit it as removed - for (int c=m_data->m_userConstraints.size()-1;c>=0;c--) - { - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.getAtIndex(c); - int userConstraintKey = m_data->m_userConstraints.getKeyAtIndex(c).getUid1(); - - if (userConstraintPtr->m_mbConstraint == mbc) - { - m_data->m_userConstraints.remove(userConstraintKey); - serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++]=userConstraintKey; - } - } - - delete mbc; - - - } - } - - if (bodyHandle->m_multiBody->getBaseCollider()) - { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()); - m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); - int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); - m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); - } - for (int link=0;linkm_multiBody->getNumLinks();link++) - { - - if (bodyHandle->m_multiBody->getLink(link).m_collider) - { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider); - m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); - int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); - m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); - } - } - int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); - m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody); - numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); - //todo: clear all other remaining data, release memory etc - - delete bodyHandle->m_multiBody; - bodyHandle->m_multiBody=0; - serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; - } - if (bodyHandle->m_rigidBody) - { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_rigidBody); - serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; - //todo: clear all other remaining data... - m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody); - int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2(); - m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); - delete bodyHandle->m_rigidBody; - bodyHandle->m_rigidBody=0; - serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; - } - } - - m_data->m_bodyHandles.freeHandle(bodyUniqueId); - } - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); - - - hasStatus = true; - break; - } - case CMD_USER_CONSTRAINT: - { - BT_PROFILE("CMD_USER_CONSTRAINT"); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; - hasStatus = true; - if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE) - { - int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid); - if (userConstraintPtr) - { - serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0; - for (int i = 0; i < 6; i++) - { - serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0; - } - if (userConstraintPtr->m_mbConstraint) - { - serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows(); - for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++) - { - serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - } - serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED; - } - } - - }; - if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO) - { - int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); - if (userConstraintPtr) - { - serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData; - - serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED; - } - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) - { - btScalar defaultMaxForce = 500.0; - InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); - if (parentBody && parentBody->m_multiBody) - { - if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) - { - InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; - //also create a constraint with just a single multibody/rigid body without child - //if (childBody) - { - btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); - btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); - btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); - btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); - - - - if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) - { - if (childBody && childBody->m_multiBody) - { - if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) - { - btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyGear->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = multibodyGear; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } - } - else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) - { - if (childBody && childBody->m_multiBody) - { - if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) - { - btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyFixed->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = multibodyFixed; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } - else - { - btRigidBody* rb = childBody? childBody->m_rigidBody : 0; - btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); - rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodyFixed); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = rigidbodyFixed; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } - else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) - { - if (childBody && childBody->m_multiBody) - { - btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - multibodySlider->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = multibodySlider; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - else - { - btRigidBody* rb = childBody? childBody->m_rigidBody : 0; - - btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodySlider); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = rigidbodySlider; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; } - - } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) - { - if (childBody && childBody->m_multiBody) - { - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); - p2p->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = p2p; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - else - { - btRigidBody* rb = childBody? childBody->m_rigidBody : 0; - - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); - p2p->setMaxAppliedImpulse(defaultMaxForce); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = p2p; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } else - { - b3Warning("unknown constraint type"); - } - - - } - } - } - else - { - InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; - - if (parentBody && childBody) - { - if (parentBody->m_rigidBody) - { - - btRigidBody* parentRb = 0; - if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1) - { - parentRb = parentBody->m_rigidBody; - } else - { - if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) && - (clientCmd.m_userConstraintArguments.m_parentJointIndexm_rigidBodyJoints.size())) - { - parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB(); - } - } - - - btRigidBody* childRb = 0; - if (childBody->m_rigidBody) - { - - if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1) - { - childRb = childBody->m_rigidBody; - } - else - { - if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0) - && (clientCmd.m_userConstraintArguments.m_childJointIndexm_rigidBodyJoints.size())) - { - childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB(); - } - - } - } - - switch (clientCmd.m_userConstraintArguments.m_jointType) - { - case eRevoluteType: - { - break; - } - case ePrismaticType: - { - break; - } - - case eFixedType: - { - if (childRb && parentRb && (childRb!=parentRb)) - { - btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); - - btTransform offsetTrA,offsetTrB; - offsetTrA.setIdentity(); - offsetTrA.setOrigin(pivotInParent); - offsetTrB.setIdentity(); - offsetTrB.setOrigin(pivotInChild); - - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB); - - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); - m_data->m_dynamicsWorld->addConstraint(dof6); - InteralUserConstraintData userConstraintData; - userConstraintData.m_rbConstraint = dof6; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - break; - } - - case ePoint2PointType: - { - if (childRb && parentRb && (childRb!=parentRb)) - { - btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); - - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild); - p2p->m_setting.m_impulseClamp = defaultMaxForce; - m_data->m_dynamicsWorld->addConstraint(p2p); - InteralUserConstraintData userConstraintData; - userConstraintData.m_rbConstraint = p2p; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - break; - } - - case eGearType: - { - - if (childRb && parentRb && (childRb!=parentRb)) - { - btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], - clientCmd.m_userConstraintArguments.m_jointAxis[1], - clientCmd.m_userConstraintArguments.m_jointAxis[2]); - //for now we use the same local axis for both objects - btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0], - clientCmd.m_userConstraintArguments.m_jointAxis[1], - clientCmd.m_userConstraintArguments.m_jointAxis[2]); - btScalar ratio=1; - btGearConstraint* gear = new btGearConstraint(*parentRb,*childRb, axisA,axisB,ratio); - m_data->m_dynamicsWorld->addConstraint(gear,true); - InteralUserConstraintData userConstraintData; - userConstraintData.m_rbConstraint = gear; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - break; - } - case eSphericalType: - { - b3Warning("constraint type not handled yet"); - break; - } - case ePlanarType: - { - b3Warning("constraint type not handled yet"); - break; - } - default: - { - b3Warning("unknown constraint type"); - } - }; - } - } - } - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) - { - serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; - int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); - if (userConstraintPtr) - { - if (userConstraintPtr->m_mbConstraint) - { - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) - { - btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], - clientCmd.m_userConstraintArguments.m_childFrame[1], - clientCmd.m_userConstraintArguments.m_childFrame[2]); - userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0]; - userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1]; - userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2]; - userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) - { - btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3], - clientCmd.m_userConstraintArguments.m_childFrame[4], - clientCmd.m_userConstraintArguments.m_childFrame[5], - clientCmd.m_userConstraintArguments.m_childFrame[6]); - userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3]; - userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4]; - userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5]; - userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6]; - btMatrix3x3 childFrameBasis(childFrameOrn); - userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) - { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; - userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; - userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) - { - userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); - userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio; - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) - { - userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget); - userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget; - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) - { - userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp); - userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp; - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) - { - userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); - userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink; - } - - } - if (userConstraintPtr->m_rbConstraint) - { - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) - { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; - userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; - //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) - { - if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) - { - btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint; - gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio); - } - } - } - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange; - serverCmd.m_updateFlags = clientCmd.m_updateFlags; - serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED; - } - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) - { - serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED; - int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); - if (userConstraintPtr) - { - if (userConstraintPtr->m_mbConstraint) - { - m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint); - delete userConstraintPtr->m_mbConstraint; - m_data->m_userConstraints.remove(userConstraintUidRemove); - } - if (userConstraintPtr->m_rbConstraint) - { - m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); - delete userConstraintPtr->m_rbConstraint; - m_data->m_userConstraints.remove(userConstraintUidRemove); - } - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; - serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; - - - } - - - } - - break; - } - case CMD_CALCULATE_INVERSE_KINEMATICS: - { - BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; - - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); - IKTrajectoryHelper* ikHelperPtr = 0; - - - if (ikHelperPtrPtr) - { - ikHelperPtr = *ikHelperPtrPtr; - } - else - { - IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; - m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); - ikHelperPtr = tmpHelper; - } - - int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; - - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) - { - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - - - btAlignedObjectArray q_current; - q_current.resize(numDofs); - - - - if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) - { - jacSize = jacobian_linear.size(); - // Set jacobian value - - - - btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); - int DofIndex = 0; - for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. - q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); - q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); - qdot[DofIndex+baseDofs] = 0; - nu[DofIndex+baseDofs] = 0; - DofIndex++; - } - } // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); - btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. - tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); - tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); - jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); - } - } - } - - - - btAlignedObjectArray q_new; - q_new.resize(numDofs); - int ikMethod = 0; - if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; - } - else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; - } - else - { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; - } - } - else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; - } - else - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS; - } - else - { - ikMethod = IK2_VEL_DLS;; - } - } - - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; - upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; - } - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } - - btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); - - btVector3DoubleData endEffectorWorldPosition; - btQuaternionDoubleData endEffectorWorldOrientation; - - btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); - btTransform endEffectorWorld; - endEffectorWorld.setOrigin(endEffectorPosWorldOrg); - endEffectorWorld.setRotation(endEffectorOriWorldOrg); - - btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); - - btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; - - btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); - - btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); - - endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); - endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - - btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); - - btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); - btTransform targetWorld; - targetWorld.setOrigin(targetPosWorld); - targetWorld.setRotation(targetOrnWorld); - btTransform targetBaseCoord; - targetBaseCoord = tr.inverse()*targetWorld; - - btVector3DoubleData targetPosBaseCoord; - btQuaternionDoubleData targetOrnBaseCoord; - targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); - targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); - - // Set joint damping coefficents. A small default - // damping constant is added to prevent singularity - // with pseudo inverse. The user can set joint damping - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) - { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; - } - } - ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); - - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); - - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;im_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); - //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); - //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; - b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; - - int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - - int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, - shapeIndex, - visualShapeStoragePtr); - if (success) { - serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; - serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; - serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; - serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; - } - hasStatus = true; - break; - } - case CMD_UPDATE_VISUAL_SHAPE: - { - BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - InternalTextureHandle* texHandle = 0; - - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); - - if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) - { - if (texHandle) - { - m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); - } - } - } - - { - int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId; - int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex; - - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (bodyHandle) - { - if (bodyHandle->m_multiBody) - { - if (linkIndex==-1) - { - if (bodyHandle->m_multiBody->getBaseCollider()) - { - int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - if (texHandle) - { - int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); - m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); - } - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) - { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) - { - m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); - } - - } - } else - { - if (linkIndexm_multiBody->getNumLinks()) - { - if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) - { - int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - if (texHandle) - { - int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); - m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); - } - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) - { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) - { - m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); - } - - } - } - } - } else - { - if (bodyHandle->m_rigidBody) - { - int graphicsIndex = bodyHandle->m_rigidBody->getUserIndex(); - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - if (texHandle) - { - int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); - m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); - } - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) - { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) - { - m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); - } - } - } - } - } + } + } //if (body && body->m_rigidBody) + } + + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; - serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; - hasStatus = true; - - break; - } - - case CMD_CHANGE_TEXTURE: - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED; - - InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId); - if(texH) - { - int gltex = texH->m_openglTextureId; - m_data->m_guiHelper->changeTexture(gltex, - (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height); - - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - } - hasStatus = true; - break; - } - case CMD_LOAD_TEXTURE: - { - BT_PROFILE("CMD_LOAD_TEXTURE"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - - char relativeFileName[1024]; - char pathPrefix[1024]; - - if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024)) - { - b3FileUtils::extractPath(relativeFileName,pathPrefix,1024); - - int texHandle = m_data->m_textureHandles.allocHandle(); - InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); - if(texH) - { - texH->m_tinyRendererTextureId = -1; - texH->m_openglTextureId = -1; - - int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); - if(uid>=0) - { - texH->m_tinyRendererTextureId = uid; - } - - { - int width,height,n; - unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3); - - if(imageData) - { - texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height); - free(imageData); - } - else - { - b3Warning("Unsupported texture image format [%s]\n",relativeFileName); - } - } - serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle; - serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; - } - } - else - { - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - } - hasStatus = true; - - break; - } - - case CMD_LOAD_BULLET: - { - BT_PROFILE("CMD_LOAD_BULLET"); - SharedMemoryStatus& serverCmd = serverStatusOut; - btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); - - const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" }; - int numPrefixes = sizeof(prefix) / sizeof(const char*); - char relativeFileName[1024]; - FILE* f = 0; - bool found = false; - - for (int i = 0; !f && iloadFile(relativeFileName); - if (ok) - { - - - int numRb = importer->getNumRigidBodies(); - serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; - - for( int i=0;igetRigidBodyByIndex(i); - if (colObj) - { - btRigidBody* rb = btRigidBody::upcast(colObj); - if (rb) - { - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - colObj->setUserIndex2(bodyUniqueId); - bodyHandle->m_rigidBody = rb; - - if (serverStatusOut.m_sdfLoadedArgs.m_numBodiesm_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); - hasStatus = true; - break; - } - } - serverCmd.m_type = CMD_BULLET_LOADING_FAILED; - hasStatus = true; - break; - } - - case CMD_SAVE_BULLET: - { - BT_PROFILE("CMD_SAVE_BULLET"); - SharedMemoryStatus& serverCmd = serverStatusOut; - - FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb"); - if (f) - { - btDefaultSerializer* ser = new btDefaultSerializer(); - m_data->m_dynamicsWorld->serialize(ser); - fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f); - fclose(f); - serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED; - delete ser; - } - serverCmd.m_type = CMD_BULLET_SAVING_FAILED; - hasStatus = true; - break; - } - - case CMD_LOAD_MJCF: - { - BT_PROFILE("CMD_LOAD_MJCF"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_MJCF_LOADING_FAILED; - const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; - int flags = CUF_USE_MJCF; - if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) - { - flags |= clientCmd.m_mjcfArguments.m_flags; - } - - bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); - if (completedOk) - { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - - serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; - int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); - for (int i=0;im_sdfRecentLoadedBodies[i]; - } - - serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED; - } else - { - serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; - } - hasStatus = true; - break; - - } - - case CMD_USER_DEBUG_DRAW: - { - BT_PROFILE("CMD_USER_DEBUG_DRAW"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; - hasStatus = true; - - - int trackingVisualShapeIndex = -1; - - if (clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId>=0) - { - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId); - if (bodyHandle) - { - int linkIndex = -1; - - if (bodyHandle->m_multiBody) - { - int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; - if (linkIndex ==-1) - { - if (bodyHandle->m_multiBody->getBaseCollider()) - { - trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); - } - } else - { - if (linkIndex >=0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) - { - if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) - { - trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); - } - } - - } - } - if (bodyHandle->m_rigidBody) - { - trackingVisualShapeIndex = bodyHandle->m_rigidBody->getUserIndex(); - } - } - } - - if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) - { - int uid = m_data->m_guiHelper->addUserDebugParameter( - clientCmd.m_userDebugDrawArgs.m_text, - clientCmd.m_userDebugDrawArgs.m_rangeMin, - clientCmd.m_userDebugDrawArgs.m_rangeMax, - clientCmd.m_userDebugDrawArgs.m_startValue - ); - serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - if (clientCmd.m_updateFlags &USER_DEBUG_READ_PARAMETER) - { - - int ok = m_data->m_guiHelper->readUserDebugParameter( - clientCmd.m_userDebugDrawArgs.m_itemUniqueId, - &serverCmd.m_userDebugDrawArgs.m_parameterValue); - if (ok) - { - serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; - } - } - if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) - { - int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body) - { - btCollisionObject* destColObj = 0; - - if (body->m_multiBody) - { - if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1) - { - destColObj = body->m_multiBody->getBaseCollider(); - } - else - { - if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks()) - { - destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider; - } - } - - } - if (body->m_rigidBody) - { - destColObj = body->m_rigidBody; - } - - if (destColObj) - { - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR) - { - destColObj->removeCustomDebugColor(); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) - { - btVector3 objectColorRGB; - objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0], - clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1], - clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]); - destColObj->setCustomDebugColor(objectColorRGB); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - } - } - } - - if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) - { - //addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;} - - int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags; - - if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION)==0) - { - optionFlags |= DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA; - } - - - int replaceItemUniqueId = -1; - if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)!=0) - { - replaceItemUniqueId = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId; - } - - - int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, - clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, - clientCmd.m_userDebugDrawArgs.m_textOrientation, - clientCmd.m_userDebugDrawArgs.m_textColorRGB, - clientCmd.m_userDebugDrawArgs.m_textSize, - clientCmd.m_userDebugDrawArgs.m_lifeTime, - trackingVisualShapeIndex, - optionFlags, - replaceItemUniqueId); - - if (uid>=0) - { - serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - } - - if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) - { - int uid = m_data->m_guiHelper->addUserDebugLine( - clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, - clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, - clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, - clientCmd.m_userDebugDrawArgs.m_lineWidth, - clientCmd.m_userDebugDrawArgs.m_lifeTime, - trackingVisualShapeIndex); - - if (uid>=0) - { - serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - } + BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } + int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) - { - m_data->m_guiHelper->removeAllUserDebugItems(); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - } - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) - { - m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks(); - } + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; - break; - } - case CMD_CUSTOM_COMMAND: - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; - serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; + if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + return hasStatus; + } - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) - { - //pluginPath could be registered or load from disk - const char* postFix = ""; - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX) - { - postFix = clientCmd.m_customCommandArgs.m_postFix; - } + //always add the base, even for static (non-moving objects) + //so that we can easily move the 'fixed' base when needed + //do we don't use this conditional "if (!mb->hasFixedBase())" + { + btTransform tr; + tr.setOrigin(mb->getBasePos()); + tr.setRotation(mb->getWorldToBaseRot().inverse()); + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = + body->m_rootLocalInertialFrame.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = + body->m_rootLocalInertialFrame.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = + body->m_rootLocalInertialFrame.getOrigin()[2]; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = + body->m_rootLocalInertialFrame.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = + body->m_rootLocalInertialFrame.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = + body->m_rootLocalInertialFrame.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = + body->m_rootLocalInertialFrame.getRotation()[3]; - int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix); - if (pluginUniqueId>=0) - { - serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; - serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; - } - } - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN) - { - m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId); - serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; - } - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND) - { - int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); - serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; - serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; - } + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - hasStatus = true; - break; - } - default: + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + } + + btAlignedObjectArray omega; + btAlignedObjectArray linVel; + + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + if (computeForwardKinematics) + { + B3_PROFILE("compForwardKinematics"); + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + world_to_local.resize(mb->getNumLinks()+1); + local_origin.resize(mb->getNumLinks()+1); + mb->forwardKinematics(world_to_local,local_origin); + } + + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); + if (computeLinkVelocities) + { + omega.resize(mb->getNumLinks()+1); + linVel.resize(mb->getNumLinks()+1); + { + B3_PROFILE("compTreeLinkVelocities"); + mb->compTreeLinkVelocities(&omega[0], &linVel[0]); + } + } + for (int l=0;lgetNumLinks();l++) + { + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; + } + for (int d=0;dgetLink(l).m_dofCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; + } + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) { - BT_PROFILE("CMD_UNKNOWN"); - b3Error("Unknown command encountered"); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; - hasStatus = true; - - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; } - }; + } else + { + btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); + btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; + } + + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + + if (supportsJointMotor(mb,l)) + { + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; + + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) + { + btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = + force; + //if (force>0) + //{ + // b3Printf("force = %f\n", force); + //} + } + } + btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); + btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); + + btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); + btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); + + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w(); + + + + btVector3 worldLinVel(0,0,0); + btVector3 worldAngVel(0,0,0); + + if (computeLinkVelocities) + { + const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); + worldLinVel = linkRotMat * linVel[l+1]; + worldAngVel = linkRotMat * omega[l+1]; + } + + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = worldLinVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = worldLinVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = worldLinVel[2]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = worldAngVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = worldAngVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = worldAngVel[2]; + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); } + + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + + } else + { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0; + + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + btTransform tr = rb->getWorldTransform(); + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + } else + { + //b3Warning("Request state but no multibody or rigid body available"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + } + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION"); + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; + + //make a snapshot of the contact manifolds into individual contact points + if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0) + { + m_data->m_cachedContactPoints.resize(0); + + int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS; + + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE) + { + mode = clientCmd.m_requestContactPointArguments.m_mode; + } + + switch (mode) + { + case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS: + { + int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds(); + m_data->m_cachedContactPoints.reserve(numContactManifolds * 4); + for (int i = 0; i < numContactManifolds; i++) + { + const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; + int linkIndexA = -1; + int linkIndexB = -1; + + int objectIndexB = -1; + const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (mblB && mblB->m_multiBody) + { + linkIndexB = mblB->m_link; + objectIndexB = mblB->m_multiBody->getUserIndex2(); + } + + int objectIndexA = -1; + const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0()); + if (bodyA) + { + objectIndexA = bodyA->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + if (mblA && mblA->m_multiBody) + { + linkIndexA = mblA->m_link; + objectIndexA = mblA->m_multiBody->getUserIndex2(); + } + btAssert(bodyA || mblA); + + //apply the filter, if the user provides it + bool swap = false; + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) + { + swap = false; + } + else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) + { + swap = true; + } + else + { + continue; + } + } + + if (swap) + { + std::swap(objectIndexA, objectIndexB); + std::swap(linkIndexA, linkIndexB); + std::swap(bodyA, bodyB); + } + + //apply the second object filter, if the user provides it + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) + { + continue; + } + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + { + continue; + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) + { + continue; + } + + for (int p = 0; p < manifold->getNumContacts(); p++) + { + + b3ContactPointData pt; + pt.m_bodyUniqueIdA = objectIndexA; + pt.m_bodyUniqueIdB = objectIndexB; + const btManifoldPoint& srcPt = manifold->getContactPoint(p); + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_linkIndexA = linkIndexA; + pt.m_linkIndexB = linkIndexB; + for (int j = 0; j < 3; j++) + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } + pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; + // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + m_data->m_cachedContactPoints.push_back(pt); + } + } + break; + } + + case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS: + { + //todo(erwincoumans) compute closest points between all, and vs all, pair + btScalar closestDistanceThreshold = 0.f; + + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD) + { + closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold; + } + + int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter; + int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter; + + bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER)); + bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER)); + + int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter; + int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter; + + btAlignedObjectArray setA; + btAlignedObjectArray setB; + btAlignedObjectArray setALinkIndex; + btAlignedObjectArray setBLinkIndex; + + if (bodyUniqueIdA >= 0) + { + InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); + if (bodyA) + { + if (bodyA->m_multiBody) + { + if (bodyA->m_multiBody->getBaseCollider()) + { + if (!hasLinkIndexAFilter || (linkIndexA == -1)) + { + setA.push_back(bodyA->m_multiBody->getBaseCollider()); + setALinkIndex.push_back(-1); + } + } + for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++) + { + if (bodyA->m_multiBody->getLink(i).m_collider) + { + if (!hasLinkIndexAFilter || (linkIndexA == i)) + { + setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); + setALinkIndex.push_back(i); + } + } + } + } + if (bodyA->m_rigidBody) + { + setA.push_back(bodyA->m_rigidBody); + setALinkIndex.push_back(-1); + } + } + } + if (bodyUniqueIdB>=0) + { + InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); + if (bodyB) + { + if (bodyB->m_multiBody) + { + if (bodyB->m_multiBody->getBaseCollider()) + { + if (!hasLinkIndexBFilter || (linkIndexB == -1)) + { + setB.push_back(bodyB->m_multiBody->getBaseCollider()); + setBLinkIndex.push_back(-1); + } + } + for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++) + { + if (bodyB->m_multiBody->getLink(i).m_collider) + { + if (!hasLinkIndexBFilter || (linkIndexB ==i)) + { + setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); + setBLinkIndex.push_back(i); + } + } + } + } + if (bodyB->m_rigidBody) + { + setB.push_back(bodyB->m_rigidBody); + setBLinkIndex.push_back(-1); + + } + } + } + + { + ///ContactResultCallback is used to report contact points + struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback + { + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + btScalar m_deltaTime; + + btAlignedObjectArray& m_cachedContactPoints; + + MyContactResultCallback(btAlignedObjectArray& pointCache) + :m_cachedContactPoints(pointCache) + { + } + + virtual ~MyContactResultCallback() + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + //bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + //collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + //return collides; + return true; + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) + { + if (cp.m_distance1<=m_closestDistanceThreshold) + { + b3ContactPointData pt; + pt.m_bodyUniqueIdA = m_bodyUniqueIdA; + pt.m_bodyUniqueIdB = m_bodyUniqueIdB; + const btManifoldPoint& srcPt = cp; + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_linkIndexA = m_linkIndexA; + pt.m_linkIndexB = m_linkIndexB; + for (int j = 0; j < 3; j++) + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } + pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; + // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + m_cachedContactPoints.push_back(pt); + } + return 1; + + } + }; + + + MyContactResultCallback cb(m_data->m_cachedContactPoints); + + cb.m_bodyUniqueIdA = bodyUniqueIdA; + cb.m_bodyUniqueIdB = bodyUniqueIdB; + cb.m_deltaTime = m_data->m_physicsDeltaTime; + + for (int i = 0; i < setA.size(); i++) + { + cb.m_linkIndexA = setALinkIndex[i]; + for (int j = 0; j < setB.size(); j++) + { + cb.m_linkIndexB = setBLinkIndex[j]; + cb.m_closestDistanceThreshold = closestDistanceThreshold; + this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb); + } + } + } + + break; + } + default: + { + b3Warning("Unknown contact query mode: %d", mode); + } + + } + } + + int numContactPoints = m_data->m_cachedContactPoints.size(); + + + //b3ContactPoint + //struct b3ContactPointDynamics + + int totalBytesPerContact = sizeof(b3ContactPointData); + int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1; + + b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; + + int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + int numContactPointBatch = btMin(numContactPoints,contactPointStorage); + + int endContactPointIndex = startContactPointIndex+numContactPointBatch; + + for (int i=startContactPointIndex;im_cachedContactPoints[i]; + b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; + destPt = srcPt; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; + } + + serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED, + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_BODY_INFO"); + + const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; + //stream info into memory + int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; + serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(sdfInfoArgs.m_bodyUniqueId); + if (bodyHandle) + { + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,bodyHandle->m_bodyName.c_str()); + } + + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + + return hasStatus; + +} + +bool PhysicsServerCommandProcessor::processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_LOAD_SDF"); + + const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true; + + int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = sdfArgs.m_globalScaling; + } + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling); + if (completedOk) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; } return hasStatus; } -//static int skip=1; +bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; + if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) + { + m_data->m_sdfRecentLoadedBodies.clear(); + + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); + + bool useMultiBody = true; + if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) + { + useMultiBody = false; + } + + int flags = 0; + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + + if (ok) + { + int bodyUniqueId = -1; + + if (m_data->m_sdfRecentLoadedBodies.size()==1) + { + bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0]; + } + m_data->m_sdfRecentLoadedBodies.clear(); + if (bodyUniqueId>=0) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + } + } + + //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; + + BT_PROFILE("CMD_LOAD_URDF"); + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); + } + btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); + btAssert(urdfArgs.m_urdfFileName); + btVector3 initialPos(0,0,0); + btQuaternion initialOrn(0,0,0,1); + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) + { + initialPos[0] = urdfArgs.m_initialPosition[0]; + initialPos[1] = urdfArgs.m_initialPosition[1]; + initialPos[2] = urdfArgs.m_initialPosition[2]; + } + int urdfFlags = 0; + if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + urdfFlags = urdfArgs.m_urdfFlags; + } + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) + { + initialOrn[0] = urdfArgs.m_initialOrientation[0]; + initialOrn[1] = urdfArgs.m_initialOrientation[1]; + initialOrn[2] = urdfArgs.m_initialOrientation[2]; + initialOrn[3] = urdfArgs.m_initialOrientation[3]; + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true; + bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false; + int bodyUniqueId; + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = urdfArgs.m_globalScaling; + } + //load the actual URDF and send a report: completed or failed + bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, + initialPos,initialOrn, + useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling); + + if (completedOk && bodyUniqueId>=0) + { + + + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + + } + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + bool hasStatus = true; +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + double scale = 0.1; + double mass = 0.1; + double collisionMargin = 0.02; + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE) + { + scale = clientCmd.m_loadBunnyArguments.m_scale; + } + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS) + { + mass = clientCmd.m_loadBunnyArguments.m_mass; + } + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN) + { + collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin; + } + m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; + m_data->m_softBodyWorldInfo.water_density = 0; + m_data->m_softBodyWorldInfo.water_offset = 0; + m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); + m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); + m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; + m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); + + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); + + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 1.0; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 50; + psb->m_cfg.kDF = 0.5; + psb->randomizeConstraints(); + psb->rotate(btQuaternion(0.70711,0,0,0.70711)); + psb->translate(btVector3(0,0,1.0)); + psb->scale(btVector3(scale,scale,scale)); + psb->setTotalMass(mass,true); + psb->getCollisionShape()->setMargin(collisionMargin); + + m_data->m_dynamicsWorld->addSoftBody(psb); + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; +#endif + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CREATE_SENSOR"); + + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_CREATE_SENSOR"); + } + int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + btAssert(mb); + for (int i=0;igetLink(jointIndex).m_jointFeedback) + { + b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); + } else + { + btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); + fb->m_reactionForces.setZero(); + mb->getLink(jointIndex).m_jointFeedback = fb; + m_data->m_multiBodyJointFeedbacks.push_back(fb); + }; + + } else + { + if (mb->getLink(jointIndex).m_jointFeedback) + { + m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); + delete mb->getLink(jointIndex).m_jointFeedback; + mb->getLink(jointIndex).m_jointFeedback=0; + } else + { + b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); + }; + + } + } + + } else + { + b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); + } + +#if 0 + //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody + /* + for (int i=0;im_dynamicsWorld->getNumConstraints();i++) + { + btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); + btJointFeedback* fb = new btJointFeedback(); + m_data->m_jointFeedbacks.push_back(fb); + c->setJointFeedback(fb); + + + } + */ +#endif + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + B3_PROFILE("custom");//clientCmd.m_profile.m_name); + { + B3_PROFILE("event");//clientCmd.m_profile.m_name); + char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name]; + char* eventName = 0; + if (eventNamePtr) + { + B3_PROFILE("reuse"); + eventName = *eventNamePtr; + + } else + { + B3_PROFILE("alloc"); + int len = strlen(clientCmd.m_profile.m_name); + eventName = new char[len+1]; + strcpy(eventName,clientCmd.m_profile.m_name); + eventName[len] = 0; + m_data->m_profileEvents.insert(eventName,eventName); + + } + + + { + { + B3_PROFILE("with");//clientCmd.m_profile.m_name); + { + B3_PROFILE("some");//clientCmd.m_profile.m_name); + { + B3_PROFILE("deep");//clientCmd.m_profile.m_name); + { + B3_PROFILE("level");//clientCmd.m_profile.m_name); + { + B3_PROFILE(eventName); + b3Clock::usleep(clientCmd.m_profile.m_durationInMicroSeconds); + } + } + } + } + } + } + } + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; + hasStatus=true; + int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; + + if (body->m_multiBody->getBaseCollider()) + { + btTransform tr; + tr.setOrigin(mb->getBasePos()); + tr.setRotation(mb->getWorldToBaseRot().inverse()); + + btVector3 aabbMin,aabbMax; + body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } + for (int l=0;lgetNumLinks();l++) + { + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = -1; + + + + if (body->m_multiBody->getLink(l).m_collider) + { + btVector3 aabbMin,aabbMax; + body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform,aabbMin,aabbMax); + + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2]; + } + + } + } + else + { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; + if (rb->getCollisionShape()) + { + btTransform tr = rb->getWorldTransform(); + + btVector3 aabbMin,aabbMax; + rb->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); + + + if (m_data->m_verboseOutput) + { + b3Printf("Step simulation request"); + b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); + } + ///todo(erwincoumans) move this damping inside Bullet + for (int i=0;im_dynamicsWorld->getNumMultibodies();i++) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); + for (int l=0;lgetNumLinks();l++) + { + for (int d=0;dgetLink(l).m_dofCount;d++) + { + double damping_coefficient = mb->getLink(l).m_jointDamping; + double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d]; + mb->addJointTorqueMultiDof(l, d, damping); + } + } + } + + btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor; + + if (m_data->m_numSimulationSubSteps > 0) + { + m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); + } + else + { + m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0); + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + return hasStatus; + +} + +bool PhysicsServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); + + //todo: also check version etc? + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_FAILED; + + int sz = btDefaultSerializer::getMemoryDnaSizeInBytes(); + const char* memDna = btDefaultSerializer::getMemoryDna(); + if (sz < bufferSizeInBytes) + { + for (int i = 0; i < sz; i++) + { + bufferServerToClient[i] = memDna[i]; + } + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; + serverCmd.m_numDataStreamBytes = sz; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO"); + + int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; + double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; + double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; + double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction; + double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction; + double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution; + btAssert(bodyUniqueId >= 0); + + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + + if (linkIndex == -1) + { + if (mb->getBaseCollider()) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getBaseCollider()->setRestitution(restitution); + } + + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getBaseCollider()->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getBaseCollider()->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getBaseCollider()->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + mb->setBaseMass(mass); + if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) + { + btVector3 localInertia; + mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia); + mb->setBaseInertia(localInertia); + } + } + } + else + { + if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) + { + if (mb->getLinkCollider(linkIndex)) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getLinkCollider(linkIndex)->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + + + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + mb->getLink(linkIndex).m_mass = mass; + if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) + { + btVector3 localInertia; + mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia); + mb->getLink(linkIndex).m_inertiaLocal = localInertia; + } + } + } + } + } else + { + if (body && body->m_rigidBody) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + btScalar angDamping = body->m_rigidBody->getAngularDamping(); + body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + btScalar linDamping = body->m_rigidBody->getLinearDamping(); + body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + body->m_rigidBody->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + body->m_rigidBody->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + body->m_rigidBody->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + body->m_rigidBody->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + btVector3 localInertia; + if (body->m_rigidBody->getCollisionShape()) + { + body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia); + } + body->m_rigidBody->setMassProps(mass,localInertia); + } + } + } + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH"); + b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path); + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; + + int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; + + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass(); + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); + } + else + { + serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex); + if (mb->getLinkCollider(linkIndex)) + { + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); + } + else + { + b3Warning("The dynamic info requested is not available"); + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; + } + } + hasStatus = true; + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED; + serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; + serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; + serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2; + serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp; + serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; + serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled(); + serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; + btVector3 grav = m_data->m_dynamicsWorld->getGravity(); + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0]; + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1]; + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; + serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; + serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps; + serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; + serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; + serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; + serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation; + serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) + { + m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; + } + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) + { + m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); + } + + //see + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) + { + //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk + gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + this->m_data->m_dynamicsWorld->setGravity(grav); + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + } + + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) + { + gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) + { + m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; + } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; + } + if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; + } + + + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) + { + b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + } + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_INIT_POSE"); + + if (m_data->m_verboseOutput) + { + b3Printf("Server Init Pose not implemented yet"); + } + int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + btVector3 baseLinVel(0, 0, 0); + btVector3 baseAngVel(0, 0, 0); + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], + clientCmd.m_initPoseArgs.m_initialStateQdot[1], + clientCmd.m_initPoseArgs.m_initialStateQdot[2]); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], + clientCmd.m_initPoseArgs.m_initialStateQdot[4], + clientCmd.m_initPoseArgs.m_initialStateQdot[5]); + } + btVector3 basePos(0, 0, 0); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + basePos = btVector3( + clientCmd.m_initPoseArgs.m_initialStateQ[0], + clientCmd.m_initPoseArgs.m_initialStateQ[1], + clientCmd.m_initPoseArgs.m_initialStateQ[2]); + } + btQuaternion baseOrn(0, 0, 0, 1); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3], + clientCmd.m_initPoseArgs.m_initialStateQ[4], + clientCmd.m_initPoseArgs.m_initialStateQ[5], + clientCmd.m_initPoseArgs.m_initialStateQ[6]); + } + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + mb->setBaseVel(baseLinVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + mb->setBaseOmega(baseAngVel); + } + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + btVector3 zero(0,0,0); + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); + + mb->setBaseVel(baseLinVel); + mb->setBasePos(basePos); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); + + mb->setBaseOmega(baseAngVel); + btQuaternion invOrn(baseOrn); + + mb->setWorldToBaseRot(invOrn.inverse()); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) + { + int uDofIndex = 6; + int posVarCountIndex = 7; + for (int i=0;igetNumLinks();i++) + { + if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount==1)) + { + mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]); + mb->setJointVel(i,0);//backwards compatibility + } + if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount==1)) + { + btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]; + mb->setJointVel(i,vel); + } + + posVarCountIndex += mb->getLink(i).m_posVarCount; + uDofIndex += mb->getLink(i).m_dofCount; + + } + } + + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + + mb->forwardKinematics(scratch_q,scratch_m); + int nLinks = mb->getNumLinks(); + scratch_q.resize(nLinks+1); + scratch_m.resize(nLinks+1); + + mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m); + + + } + + if (body && body->m_rigidBody) + { + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + body->m_rigidBody->setLinearVelocity(baseLinVel); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + body->m_rigidBody->setAngularVelocity(baseAngVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + body->m_rigidBody->getWorldTransform().setOrigin(basePos); + body->m_rigidBody->setLinearVelocity(baseLinVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + body->m_rigidBody->getWorldTransform().setRotation(baseOrn); + body->m_rigidBody->setAngularVelocity(baseAngVel); + } + + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_RESET_SIMULATION"); + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); + resetSimulation(); + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; + + BT_PROFILE("CMD_CREATE_RIGID_BODY"); + + btVector3 halfExtents(1,1,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) + { + halfExtents = btVector3( + clientCmd.m_createBoxShapeArguments.m_halfExtentsX, + clientCmd.m_createBoxShapeArguments.m_halfExtentsY, + clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); + } + btTransform startTrans; + startTrans.setIdentity(); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) + { + startTrans.setOrigin(btVector3( + clientCmd.m_createBoxShapeArguments.m_initialPosition[0], + clientCmd.m_createBoxShapeArguments.m_initialPosition[1], + clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); + } + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) + { + + startTrans.setRotation(btQuaternion( + clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); + } + + btScalar mass = 0.f; + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) + { + mass = clientCmd.m_createBoxShapeArguments.m_mass; + } + + int shapeType = COLLISION_SHAPE_TYPE_BOX; + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) + { + shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; + } + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + m_data->m_worldImporters.push_back(worldImporter); + + btCollisionShape* shape = 0; + + switch (shapeType) + { + case COLLISION_SHAPE_TYPE_CYLINDER_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCylinderShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCylinderShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCylinderShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCapsuleShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCapsuleShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCapsuleShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_SPHERE: + { + btScalar radius = halfExtents[0]; + shape = worldImporter->createSphereShape(radius); + break; + } + case COLLISION_SHAPE_TYPE_BOX: + default: + { + shape = worldImporter->createBoxShape(halfExtents); + } + } + + + bool isDynamic = (mass>0); + btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); + //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + btVector4 colorRGBA(1,0,0,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) + { + colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; + colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; + colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; + colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; + } + m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); + m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); + + + int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; + rb->setUserIndex2(bodyUniqueId); + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + bodyHandle->m_rigidBody = rb; + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processPickBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_PICK_BODY"); + + pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processMovePickedBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_MOVE_PICKED_BODY"); + + movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRemovePickingConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY"); + removePickingConstraint(); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestAabbOverlapCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_REQUEST_AABB_OVERLAP"); + SharedMemoryStatus& serverCmd = serverStatusOut; + int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + + if (0== curObjectIndex) + { + //clientCmd.m_requestContactPointArguments.m_aabbQueryMin + btVector3 aabbMin, aabbMax; + aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]); + aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]); + + m_data->m_cachedOverlappingObjects.clear(); + + m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects); + } + + + int totalBytesPerObject = sizeof(b3OverlappingObject); + int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1; + int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); + int remainingObjects = numOverlap - curObjectIndex; + + int curNumObjects = btMin(overlapCapacity, remainingObjects); + + if (numOverlap < overlapCapacity) + { + + b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient; + for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++) + { + overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i]; + overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i]; + } + + serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED; + + //int m_startingOverlappingObjectIndex; + //int m_numOverlappingObjectsCopied; + //int m_numRemainingOverlappingObjects; + serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); + serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects; + } + else + { + serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestOpenGLVisualizeCameraCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_REQUEST_OPENGL_VISUALIZER_CAMERA"); + SharedMemoryStatus& serverCmd = serverStatusOut; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &serverCmd.m_visualizerCameraResultArgs.m_width, + &serverCmd.m_visualizerCameraResultArgs.m_height, + serverCmd.m_visualizerCameraResultArgs.m_viewMatrix, + serverCmd.m_visualizerCameraResultArgs.m_projectionMatrix, + serverCmd.m_visualizerCameraResultArgs.m_camUp, + serverCmd.m_visualizerCameraResultArgs.m_camForward, + serverCmd.m_visualizerCameraResultArgs.m_horizontal, + serverCmd.m_visualizerCameraResultArgs.m_vertical, + &serverCmd.m_visualizerCameraResultArgs.m_yaw, + &serverCmd.m_visualizerCameraResultArgs.m_pitch, + &serverCmd.m_visualizerCameraResultArgs.m_dist, + serverCmd.m_visualizerCameraResultArgs.m_target); + serverCmd.m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type =CMD_CLIENT_COMMAND_COMPLETED; + + hasStatus = true; + + if (clientCmd.m_updateFlags&COV_SET_FLAGS) + { + if (clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag == COV_ENABLE_TINY_RENDERER) + { + m_data->m_enableTinyRenderer = clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled!=0; + } + m_data->m_guiHelper->setVisualizerFlag(clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag, + clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled); + } + if (clientCmd.m_updateFlags&COV_SET_CAMERA_VIEW_MATRIX) + { + m_data->m_guiHelper->resetCamera( clientCmd.m_configureOpenGLVisualizerArguments.m_cameraDistance, + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraYaw, + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraPitch, + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0], + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1], + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]); + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS"); + SharedMemoryStatus& serverCmd = serverStatusOut; + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); + for (int i = 0; i < num_dofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; + qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; + nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; + } + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; + for (int i = 0; i < num_dofs; i++) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs]; + } + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCalculateJacobianCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CALCULATE_JACOBIAN"); + + SharedMemoryStatus& serverCmd = serverStatusOut; + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx q(numDofs + baseDofs); + btInverseDynamics::vecx qdot(numDofs + baseDofs); + btInverseDynamics::vecx nu(numDofs + baseDofs); + btInverseDynamics::vecx joint_force(numDofs + baseDofs); + for (int i = 0; i < numDofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i]; + qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i]; + nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; + } + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs; + // Set jacobian value + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); + btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs); + + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t); + tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r); + // Update the translational jacobian based on the desired local point. + // v_pt = v_frame + w x pt + // v_pt = J_t * qd + (J_r * qd) x pt + // v_pt = J_t * qd - pt x (J_r * qd) + // v_pt = J_t * qd - pt_x * J_r * qd) + // v_pt = (J_t - pt_x * J_r) * qd + // J_t_new = J_t - pt_x * J_r + btInverseDynamics::vec3 localPosition; + for (int i = 0; i < 3; ++i) { + localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i]; + } + // Only calculate if the localPosition is non-zero. + if (btInverseDynamics::maxAbs(localPosition) > 0.0) { + // Write the localPosition into world coordinates. + btInverseDynamics::mat33 world_rotation_body; + tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body); + localPosition = world_rotation_body * localPosition; + // Correct the translational jacobian. + btInverseDynamics::mat33 skewCrossProduct; + btInverseDynamics::skew(localPosition, &skewCrossProduct); + btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs); + btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l); + btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs); + btInverseDynamics::sub(jac_t, jac_l, &jac_t_new); + jac_t = jac_t_new; + } + // Fill in the result into the shared memory. + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < (numDofs + baseDofs); ++j) + { + int element = (numDofs + baseDofs) * i + j; + serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j); + serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j); + } + } + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; + } + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; + } + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CALCULATE_MASS_MATRIX"); + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + const int totDofs = numDofs + baseDofs; + btInverseDynamics::vecx q(totDofs); + btInverseDynamics::matxx massMatrix(totDofs, totDofs); + for (int i = 0; i < numDofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i]; + } + if (-1 != tree->calculateMassMatrix(q, &massMatrix)) + { + serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; + // Fill in the result into the shared memory. + double* sharedBuf = (double*)bufferServerToClient; + int sizeInBytes = totDofs*totDofs*sizeof(double); + if (sizeInBytes < bufferSizeInBytes) + { + for (int i = 0; i < (totDofs); ++i) + { + for (int j = 0; j < (totDofs); ++j) + { + int element = (totDofs) * i + j; + + sharedBuf[element] = massMatrix(i,j); + } + } + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; + } + } + + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; + } + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE"); + + if (m_data->m_verboseOutput) + { + b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber); + } + for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) + { + InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) + { + btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + btVector3 tmpPosition( + clientCmd.m_externalForceArguments.m_positions[i*3+0], + clientCmd.m_externalForceArguments.m_positions[i*3+1], + clientCmd.m_externalForceArguments.m_positions[i*3+2]); + + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpForce : tmpForce; + btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + mb->addBaseForce(forceWorld); + mb->addBaseTorque(relPosWorld.cross(forceWorld)); + //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + + btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpForce : tmpForce; + btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + mb->addLinkForce(link, forceWorld); + mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); + //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); + } + } + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; + mb->addBaseTorque(torqueWorld); + //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; + mb->addLinkTorque(link, torqueWorld); + //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } + } + } + + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); + + btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal; + rb->applyForce(forceWorld, relPosWorld); + + } + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + + btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal; + rb->applyTorque(torqueWorld); + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_REMOVE_BODY_FAILED; + serverCmd.m_removeObjectArgs.m_numBodies = 0; + serverCmd.m_removeObjectArgs.m_numUserConstraints = 0; + + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); + + for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); + if (bodyHandle) + { + if (bodyHandle->m_multiBody) + { + serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + + //also remove user constraints... + for (int i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--) + { + btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); + if ((mbc->getMultiBodyA() == bodyHandle->m_multiBody)||(mbc->getMultiBodyB()==bodyHandle->m_multiBody)) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbc); + + //also remove user constraint and submit it as removed + for (int c=m_data->m_userConstraints.size()-1;c>=0;c--) + { + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.getAtIndex(c); + int userConstraintKey = m_data->m_userConstraints.getKeyAtIndex(c).getUid1(); + + if (userConstraintPtr->m_mbConstraint == mbc) + { + m_data->m_userConstraints.remove(userConstraintKey); + serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++]=userConstraintKey; + } + } + + delete mbc; + + + } + } + + if (bodyHandle->m_multiBody->getBaseCollider()) + { + m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()); + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); + int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); + } + for (int link=0;linkm_multiBody->getNumLinks();link++) + { + + if (bodyHandle->m_multiBody->getLink(link).m_collider) + { + m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider); + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); + int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); + } + } + int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); + m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody); + numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); + //todo: clear all other remaining data, release memory etc + + delete bodyHandle->m_multiBody; + bodyHandle->m_multiBody=0; + serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; + } + if (bodyHandle->m_rigidBody) + { + m_data->m_visualConverter.removeVisualShape(bodyHandle->m_rigidBody); + serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + //todo: clear all other remaining data... + m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody); + int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); + delete bodyHandle->m_rigidBody; + bodyHandle->m_rigidBody=0; + serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; + } + } + + m_data->m_bodyHandles.freeHandle(bodyUniqueId); + } + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_USER_CONSTRAINT"); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; + hasStatus = true; + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE) + { + int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid); + if (userConstraintPtr) + { + serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0; + for (int i = 0; i < 6; i++) + { + serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0; + } + if (userConstraintPtr->m_mbConstraint) + { + serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows(); + for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++) + { + serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + } + serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED; + } + } + + }; + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO) + { + int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); + if (userConstraintPtr) + { + serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData; + + serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED; + } + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) + { + btScalar defaultMaxForce = 500.0; + InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); + if (parentBody && parentBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) + { + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + //also create a constraint with just a single multibody/rigid body without child + //if (childBody) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); + btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); + btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); + + + + if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + { + if (childBody && childBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) + { + btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyGear->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyGear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + } + else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) + { + if (childBody && childBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) + { + btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyFixed->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyFixed; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + else + { + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); + rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodyFixed); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = rigidbodyFixed; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) + { + if (childBody && childBody->m_multiBody) + { + btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + multibodySlider->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodySlider; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + else + { + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodySlider); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = rigidbodySlider; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; } + + } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) + { + if (childBody && childBody->m_multiBody) + { + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); + p2p->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + else + { + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); + p2p->setMaxAppliedImpulse(defaultMaxForce); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } else + { + b3Warning("unknown constraint type"); + } + + + } + } + } + else + { + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + + if (parentBody && childBody) + { + if (parentBody->m_rigidBody) + { + + btRigidBody* parentRb = 0; + if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1) + { + parentRb = parentBody->m_rigidBody; + } else + { + if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) && + (clientCmd.m_userConstraintArguments.m_parentJointIndexm_rigidBodyJoints.size())) + { + parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB(); + } + } + + + btRigidBody* childRb = 0; + if (childBody->m_rigidBody) + { + + if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1) + { + childRb = childBody->m_rigidBody; + } + else + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0) + && (clientCmd.m_userConstraintArguments.m_childJointIndexm_rigidBodyJoints.size())) + { + childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB(); + } + + } + } + + switch (clientCmd.m_userConstraintArguments.m_jointType) + { + case eRevoluteType: + { + break; + } + case ePrismaticType: + { + break; + } + + case eFixedType: + { + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + + btTransform offsetTrA,offsetTrB; + offsetTrA.setIdentity(); + offsetTrA.setOrigin(pivotInParent); + offsetTrB.setIdentity(); + offsetTrB.setOrigin(pivotInChild); + + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB); + + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + m_data->m_dynamicsWorld->addConstraint(dof6); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = dof6; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + break; + } + + case ePoint2PointType: + { + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild); + p2p->m_setting.m_impulseClamp = defaultMaxForce; + m_data->m_dynamicsWorld->addConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + break; + } + + case eGearType: + { + + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + //for now we use the same local axis for both objects + btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + btScalar ratio=1; + btGearConstraint* gear = new btGearConstraint(*parentRb,*childRb, axisA,axisB,ratio); + m_data->m_dynamicsWorld->addConstraint(gear,true); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = gear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + break; + } + case eSphericalType: + { + b3Warning("constraint type not handled yet"); + break; + } + case ePlanarType: + { + b3Warning("constraint type not handled yet"); + break; + } + default: + { + b3Warning("unknown constraint type"); + } + }; + } + } + } + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) + { + serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; + int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], + clientCmd.m_userConstraintArguments.m_childFrame[1], + clientCmd.m_userConstraintArguments.m_childFrame[2]); + userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0]; + userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1]; + userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2]; + userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3], + clientCmd.m_userConstraintArguments.m_childFrame[4], + clientCmd.m_userConstraintArguments.m_childFrame[5], + clientCmd.m_userConstraintArguments.m_childFrame[6]); + userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3]; + userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4]; + userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5]; + userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6]; + btMatrix3x3 childFrameBasis(childFrameOrn); + userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; + userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio; + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget); + userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget; + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp); + userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp; + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); + userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink; + } + + } + if (userConstraintPtr->m_rbConstraint) + { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; + //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) + { + btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint; + gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + } + } + } + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange; + serverCmd.m_updateFlags = clientCmd.m_updateFlags; + serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED; + } + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) + { + serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED; + int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint); + delete userConstraintPtr->m_mbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); + } + if (userConstraintPtr->m_rbConstraint) + { + m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); + delete userConstraintPtr->m_rbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); + } + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; + serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; + + + } + + + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); + IKTrajectoryHelper* ikHelperPtr = 0; + + + if (ikHelperPtrPtr) + { + ikHelperPtr = *ikHelperPtrPtr; + } + else + { + IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; + m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); + ikHelperPtr = tmpHelper; + } + + int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; + + + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + { + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + b3AlignedObjectArray jacobian_linear; + jacobian_linear.resize(3*numDofs); + b3AlignedObjectArray jacobian_angular; + jacobian_angular.resize(3*numDofs); + int jacSize = 0; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + + + btAlignedObjectArray q_current; + q_current.resize(numDofs); + + + + if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) + { + jacSize = jacobian_linear.size(); + // Set jacobian value + + + + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { + if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. + q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); + q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); + qdot[DofIndex+baseDofs] = 0; + nu[DofIndex+baseDofs] = 0; + DofIndex++; + } + } // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); + btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < numDofs; ++j) + { + jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); + jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); + } + } + } + + + + btAlignedObjectArray q_new; + q_new.resize(numDofs); + int ikMethod = 0; + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + } + else + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; + } + else + { + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS; + } + else + { + ikMethod = IK2_VEL_DLS;; + } + } + + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + } + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } + + btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + + btVector3DoubleData endEffectorWorldPosition; + btQuaternionDoubleData endEffectorWorldOrientation; + + btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); + btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); + btTransform endEffectorWorld; + endEffectorWorld.setOrigin(endEffectorPosWorldOrg); + endEffectorWorld.setRotation(endEffectorOriWorldOrg); + + btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + + btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; + + btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); + + btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); + + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); + + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); + btTransform targetWorld; + targetWorld.setOrigin(targetPosWorld); + targetWorld.setRotation(targetOrnWorld); + btTransform targetBaseCoord; + targetBaseCoord = tr.inverse()*targetWorld; + + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) + { + for (int i = 0; i < numDofs; ++i) + { + joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + } + } + ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); + + double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; + ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); + + serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + for (int i=0;im_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); + //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); + //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; + b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; + + int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + + int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, + shapeIndex, + visualShapeStoragePtr); + if (success) { + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; + serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; + serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; + InternalTextureHandle* texHandle = 0; + + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + + if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) + { + if (texHandle) + { + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); + } + } + } + + { + int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId; + int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (bodyHandle) + { + if (bodyHandle->m_multiBody) + { + if (linkIndex==-1) + { + if (bodyHandle->m_multiBody->getBaseCollider()) + { + int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + + } + } else + { + if (linkIndexm_multiBody->getNumLinks()) + { + if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) + { + int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + + } + } + } + } else + { + if (bodyHandle->m_rigidBody) + { + int graphicsIndex = bodyHandle->m_rigidBody->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + } + } + } + } + + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED; + + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId); + if(texH) + { + int gltex = texH->m_openglTextureId; + m_data->m_guiHelper->changeTexture(gltex, + (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height); + + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_LOAD_TEXTURE"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + + char relativeFileName[1024]; + char pathPrefix[1024]; + + if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024)) + { + b3FileUtils::extractPath(relativeFileName,pathPrefix,1024); + + int texHandle = m_data->m_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = -1; + texH->m_openglTextureId = -1; + + int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); + if(uid>=0) + { + texH->m_tinyRendererTextureId = uid; + } + + { + int width,height,n; + unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3); + + if(imageData) + { + texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height); + free(imageData); + } + else + { + b3Warning("Unsupported texture image format [%s]\n",relativeFileName); + } + } + serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle; + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + } + } + else + { + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + BT_PROFILE("CMD_LOAD_BULLET"); + + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_BULLET_LOADING_FAILED; + + btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); + + const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" }; + int numPrefixes = sizeof(prefix) / sizeof(const char*); + char relativeFileName[1024]; + FILE* f = 0; + bool found = false; + + for (int i = 0; !f && iloadFile(relativeFileName); + if (ok) + { + + + int numRb = importer->getNumRigidBodies(); + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + + for( int i=0;igetRigidBodyByIndex(i); + if (colObj) + { + btRigidBody* rb = btRigidBody::upcast(colObj); + if (rb) + { + int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + colObj->setUserIndex2(bodyUniqueId); + bodyHandle->m_rigidBody = rb; + + if (serverStatusOut.m_sdfLoadedArgs.m_numBodiesm_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_LOAD_MJCF"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_MJCF_LOADING_FAILED; + const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; + int flags = CUF_USE_MJCF; + if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + flags |= clientCmd.m_mjcfArguments.m_flags; + } + + bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); + if (completedOk) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; + } + return hasStatus; + +} + + +bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SAVE_BULLET"); + SharedMemoryStatus& serverCmd = serverStatusOut; + + FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb"); + if (f) + { + btDefaultSerializer* ser = new btDefaultSerializer(); + m_data->m_dynamicsWorld->serialize(ser); + fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f); + fclose(f); + serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED; + delete ser; + } + serverCmd.m_type = CMD_BULLET_SAVING_FAILED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) +{ + // BT_PROFILE("processCommand"); + + int sz = sizeof(SharedMemoryStatus); + int sz2 = sizeof(SharedMemoryCommand); + + bool hasStatus = false; + + if (m_data->m_commandLogger) + { + m_data->m_commandLogger->logCommand(clientCmd); + } + serverStatusOut.m_type = CMD_INVALID_STATUS; + serverStatusOut.m_numDataStreamBytes = 0; + serverStatusOut.m_dataStream = 0; + + //consume the command + switch (clientCmd.m_type) + { + + case CMD_STATE_LOGGING: + { + hasStatus = processStateLoggingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_VR_CAMERA_STATE: + { + hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VR_EVENTS_DATA: + { + hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_MOUSE_EVENTS_DATA: + { + hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_KEYBOARD_EVENTS_DATA: + { + hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS: + { + + hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_DEBUG_LINES: + { + hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SYNC_BODY_INFO: + { + hasStatus = processSyncBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_WORLD: + { + hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SDF: + { + hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_ADDITIONAL_SEARCH_PATH: + { + hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_URDF: + { + hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_BUNNY: + { + hasStatus = processLoadBunnyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_SENSOR: + { + hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PROFILE_TIMING: + { + hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_DESIRED_STATE: + { + hasStatus = processSendDesiredStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_INFO: + { + hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_ACTUAL_STATE: + { + hasStatus = processRequestActualStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + hasStatus = processForwardDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_INTERNAL_DATA: + { + hasStatus = processRequestInternalDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_CHANGE_DYNAMICS_INFO: + { + hasStatus = processChangeDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_GET_DYNAMICS_INFO: + { + hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processSendPhysicsParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + + }; + case CMD_INIT_POSE: + { + hasStatus = processInitPoseCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_RESET_SIMULATION: + { + hasStatus = processResetSimulationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_RIGID_BODY: + { + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PICK_BODY: + { + hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_MOVE_PICKED_BODY: + { + hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_AABB_OVERLAP: + { + hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: + { + hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CONFIGURE_OPENGL_VISUALIZER: + { + hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_JACOBIAN: + { + hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_MASS_MATRIX: + { + hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_APPLY_EXTERNAL_FORCE: + { + hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_BODY: + { + hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_CONSTRAINT: + { + hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_KINEMATICS: + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_UPDATE_VISUAL_SHAPE: + { + hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CHANGE_TEXTURE: + { + hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_TEXTURE: + { + hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_BULLET: + { + hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_BULLET: + { + hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_DEBUG_DRAW: + { + hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CUSTOM_COMMAND: + { + hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + default: + { + BT_PROFILE("CMD_UNKNOWN"); + b3Error("Unknown command encountered"); + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + } + }; + + return hasStatus; +} void PhysicsServerCommandProcessor::syncPhysicsToGraphics() { m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); } - void PhysicsServerCommandProcessor::renderScene(int renderFlags) { if (m_data->m_guiHelper) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index d46203419..6177d78fe 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -25,6 +25,61 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface protected: + bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSaveWorldCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestVREventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestMouseEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestKeyboardEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestDebugLinesCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processGetDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateRigidBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processPickBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processMovePickedBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemovePickingConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestAabbOverlapCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestOpenGLVisualizeCameraCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processConfigureOpenGLVisualizerCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCalculateJacobianCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCalculateMassMatrixCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processApplyExternalForceCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemoveBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling); From 13937b80f8b131c1191720eb664e8fbde9b65410 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 17 Nov 2017 18:09:42 -0800 Subject: [PATCH 09/30] bump up version of pybullet to 1.6.7 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 51ae4f5fa..81316647b 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.6.6', + version='1.6.7', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 81146a4090982e593b7a1318c78622d974d456cf Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 18 Nov 2017 13:21:20 -0800 Subject: [PATCH 10/30] pybullet: move RGB,depth, segmentation mask preview windows to the right remove some dead code from PhysicsServerCommandProcessor.cpp --- .../Common2dCanvasInterface.h | 2 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 13 +- .../PhysicsServerCommandProcessor.cpp | 259 +----------------- .../SharedMemory/PhysicsServerExample.cpp | 16 +- 4 files changed, 17 insertions(+), 273 deletions(-) diff --git a/examples/CommonInterfaces/Common2dCanvasInterface.h b/examples/CommonInterfaces/Common2dCanvasInterface.h index d531de2b1..b13dce97c 100644 --- a/examples/CommonInterfaces/Common2dCanvasInterface.h +++ b/examples/CommonInterfaces/Common2dCanvasInterface.h @@ -4,7 +4,7 @@ struct Common2dCanvasInterface { virtual ~Common2dCanvasInterface() {} - virtual int createCanvas(const char* canvasName, int width, int height)=0; + virtual int createCanvas(const char* canvasName, int width, int height, int xPos, int yPos)=0; virtual void destroyCanvas(int canvasId)=0; virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0; virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)=0; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index d0d14b200..62cd73147 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -732,12 +732,11 @@ struct QuickCanvas : public Common2dCanvasInterface MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS]; GraphingTexture* m_gt[MAX_GRAPH_WINDOWS]; int m_curNumGraphWindows; - int m_curXpos; + QuickCanvas(GL3TexLoader* myTexLoader) :m_myTexLoader(myTexLoader), - m_curNumGraphWindows(0), - m_curXpos(0) + m_curNumGraphWindows(0) { for (int i=0;igetInternalData()); input.m_width=width; input.m_height=height; - input.m_xPos = m_curXpos;//GUI will clamp it to the right//300; - m_curXpos+=width+20; - input.m_yPos = 10000;//GUI will clamp it to bottom + input.m_xPos = xPos; + input.m_yPos = yPos; input.m_name=canvasName; input.m_texName = canvasName; m_gt[slot] = new GraphingTexture; @@ -778,7 +776,6 @@ struct QuickCanvas : public Common2dCanvasInterface } virtual void destroyCanvas(int canvasId) { - m_curXpos = 0; btAssert(canvasId>=0); delete m_gt[canvasId]; m_gt[canvasId] = 0; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 44ed1eb88..b96177ddc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -60,9 +60,6 @@ #endif -//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! -btVector3 gLastPickPos(0, 0, 0); - int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -1604,49 +1601,6 @@ struct PhysicsServerCommandProcessorInternalData m_userVisualShapeHandles.exitHandles(); m_userVisualShapeHandles.initHandles(); -#if 0 - btAlignedObjectArray bla; - - for (int i=0;i<1024;i++) - { - int handle = allocHandle(); - bla.push_back(handle); - InternalBodyHandle* body = getHandle(handle); - InternalBodyData* body2 = body; - } - for (int i=0;im_enableTinyRenderer); bool loadOk = u2b.loadURDF(fileName, useFixedBase); - if (loadOk) { - -#if 1 btTransform rootTrans; rootTrans.setOrigin(pos); rootTrans.setRotation(orn); @@ -2762,188 +2713,11 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (m_data->m_sdfRecentLoadedBodies.size()==1) { *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0]; - } m_data->m_sdfRecentLoadedBodies.clear(); } return ok; -#else - - //get a body index - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - if (bodyUniqueIdPtr) - *bodyUniqueIdPtr= bodyUniqueId; - - //quick prototype of 'save world' for crude world editing - { - SaveWorldObjectData sd; - sd.m_fileName = fileName; - sd.m_bodyUniqueIds.push_back(bodyUniqueId); - m_data->m_saveWorldBodyData.push_back(sd); - } - - u2b.setBodyUniqueId(bodyUniqueId); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - - - { - btScalar mass = 0; - bodyHandle->m_rootLocalInertialFrame.setIdentity(); - btVector3 localInertiaDiagonal(0,0,0); - int urdfLinkIndex = u2b.getRootLinkIndex(); - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); - } - if (m_data->m_verboseOutput) - { - b3Printf("loaded %s OK!", fileName); - } - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(pos); - tr.setRotation(orn); - //int rootLinkIndex = u2b.getRootLinkIndex(); - // printf("urdf root link index = %d\n",rootLinkIndex); - MyMultiBodyCreator creation(m_data->m_guiHelper); - - flags |= URDF_ORDER_TYPED_CONSTRAINT; - - ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); - - for (int i=0;im_collisionShapes.push_back(shape); - } - - btMultiBody* mb = creation.getBulletMultiBody(); - btRigidBody* rb = creation.getRigidBody(); - - bodyHandle->m_bodyName = u2b.getBodyName(); - - if (useMultiBody) - { - - - if (mb) - { - mb->setUserIndex2(bodyUniqueId); - bodyHandle->m_multiBody = mb; - - if (flags & URDF_USE_SELF_COLLISION) - { - mb->setHasSelfCollision(true); - } - createJointMotors(mb); - -#ifdef B3_ENABLE_TINY_AUDIO - { - SDFAudioSource audioSource; - int urdfRootLink = u2b.getRootLinkIndex();//LinkIndex = creation.m_mb2urdfLink[-1]; - if (u2b.getLinkAudioSource(urdfRootLink,audioSource)) - { - int flags = mb->getBaseCollider()->getCollisionFlags(); - mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); - audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); - if (audioSource.m_userIndex>=0) - { - bodyHandle->m_audioSources.insert(-1, audioSource); - } - } - } -#endif - - //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire - UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; - m_data->m_urdfLinkNameMapper.push_back(util); - util->m_mb = mb; - for (int i = 0; i < bufferSizeInBytes; i++) - { - bufferServerToClient[i] = 0;//0xcc; - } - util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); - //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); - util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); - - util->m_memSerializer->startSerialization(); - - - bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); - for (int i=0;igetNumLinks();i++) - { - int link=i; - //disable serialization of the collision objects - util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); - int urdfLinkIndex = creation.m_mb2urdfLink[i]; - btScalar mass; - btVector3 localInertiaDiagonal(0,0,0); - btTransform localInertialFrame; - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); - bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); - - std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); - m_data->m_strings.push_back(linkName); - util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str()); - mb->getLink(i).m_linkName = linkName->c_str(); - - std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); - m_data->m_strings.push_back(jointName); - util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); - mb->getLink(i).m_jointName = jointName->c_str(); -#ifdef B3_ENABLE_TINY_AUDIO - { - SDFAudioSource audioSource; - int urdfLinkIndex = creation.m_mb2urdfLink[link]; - if (u2b.getLinkAudioSource(urdfLinkIndex,audioSource)) - { - int flags = mb->getLink(link).m_collider->getCollisionFlags(); - mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); - audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); - if (audioSource.m_userIndex>=0) - { - bodyHandle->m_audioSources.insert(link, audioSource); - } - } - } -#endif - } - - std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); - m_data->m_strings.push_back(baseName); - - mb->setBaseName(baseName->c_str()); - - util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); - - - - int len = mb->calculateSerializeBufferSize(); - btChunk* chunk = util->m_memSerializer->allocate(len,1); - const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); - util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - - - - return true; - } else - { - b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); - return false; - } - - } else - { - if (rb) - { - bodyHandle->m_rigidBody = rb; - rb->setUserIndex2(bodyUniqueId); - return true; - } - } - #endif } - return false; } @@ -3017,14 +2791,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* for (int i=0;im_rigidBodyJoints.size();i++) { const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i); -#if 0 - const btRigidBody& bodyA = con->getRigidBodyA(); - const btRigidBody& bodyB = con->getRigidBodyB(); - int len = bodyA.calculateSerializeBufferSize(); - btChunk* chunk = util->m_memSerializer->allocate(len,1); - const char* structType = bodyA.serialize(chunk->m_oldPtr, util->m_memSerializer); - util->m_memSerializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)&bodyA); -#endif + util->m_memSerializer->registerNameForPointer(con,bodyHandle->m_rigidBodyJointNames[i].c_str()); util->m_memSerializer->registerNameForPointer(&con->getRigidBodyB(),bodyHandle->m_rigidBodyLinkNames[i].c_str()); @@ -3037,27 +2804,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* } streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); -#if 0 - util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); - if (mb->getBaseName()) - { - util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); - } - bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); - for (int i=0;igetNumLinks();i++) - { - //disable serialization of the collision objects - util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); - util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName); - util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName); - } - util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); - int len = mb->calculateSerializeBufferSize(); - btChunk* chunk = util->m_memSerializer->allocate(len,1); - const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); - util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); -#endif + } } @@ -8847,7 +8594,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons { btVector3 pickPos = rayCallback.m_hitPointWorld; - gLastPickPos = pickPos; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); if (body) { diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 4f051e42b..32ce41d36 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -30,8 +30,8 @@ bool gActivedVRRealTimeSimulation = false; bool gEnableSyncPhysicsRendering= true; bool gEnableUpdateDebugDrawLines = true; -static int gCamVisualizerWidth = 320; -static int gCamVisualizerHeight = 240; +static int gCamVisualizerWidth = 228; +static int gCamVisualizerHeight = 192; static bool gEnableDefaultKeyboardShortcuts = true; static bool gEnableDefaultMousePicking = true; @@ -1751,9 +1751,9 @@ void PhysicsServerExample::initPhysics() { - m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight); - m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight); - m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight); + m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight, 8,55); + m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight,8,75+gCamVisualizerHeight); + m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight,8,95+gCamVisualizerHeight*2); for (int i=0;icreateCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight); + m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight, 8,55); } } else { @@ -1922,7 +1922,7 @@ void PhysicsServerExample::updateGraphics() { if (m_canvasDepthIndex<0) { - m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight); + m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight,8,75+gCamVisualizerHeight); } } else { @@ -1940,7 +1940,7 @@ void PhysicsServerExample::updateGraphics() { if (m_canvasSegMaskIndex<0) { - m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight); + m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight,8,95+gCamVisualizerHeight*2); } } else { From 099a6f65f314dee885e61e221f5791586d8e9301 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 18 Nov 2017 15:18:46 -0800 Subject: [PATCH 11/30] tweak zbuffer visualization, show targetpos in example browser --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 2 +- .../SharedMemory/PhysicsServerExample.cpp | 24 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 62cd73147..4fa27685c 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1326,7 +1326,7 @@ void OpenGLExampleBrowser::update(float deltaTime) float camPos[3]; s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(camPos); s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget); - sprintf(msg,"camPos=%f,%f,%f, dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]); + sprintf(msg,"camTargetPos=%2.2f,%2.2f,%2.2f, dist=%2.2f, pitch=%2.2f, yaw=%2.2f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw); gui2->setStatusBarMessage(msg, true); } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 32ce41d36..ebadcad29 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -2096,8 +2096,9 @@ void PhysicsServerExample::updateGraphics() int startSegIndex = m_multiThreadedHelper->m_startPixelIndex; int endSegIndex = startSegIndex + (*m_multiThreadedHelper->m_numPixelsCopied); - btScalar frustumZNear = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]-1); - btScalar frustumZFar = 20;//m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]+1); + //btScalar frustumZNear = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]-1); + //btScalar frustumZFar = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]+1); + for (int i=0;i-1e20) { int rgb = 0; - btScalar minDepthValue = 0.98;//todo: compute more reasonably min/max depth range - btScalar maxDepthValue = 1; + btScalar frustumZNear = 0.1; + btScalar frustumZFar = 30; + btScalar minDepthValue = frustumZNear;//todo: compute more reasonably min/max depth range + btScalar maxDepthValue = frustumZFar; - if (maxDepthValue!=minDepthValue) - { - rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue))); - if (rgb<0 || rgb>255) - { - //printf("rgb=%d\n",rgb); - } - } + float depth = depthValue; + double linearDepth = 255.*(2.0 * frustumZNear) / (frustumZFar + frustumZNear - depth * (frustumZFar - frustumZNear)); + btClamp(linearDepth, btScalar(0),btScalar(255)); + rgb = linearDepth; + m_canvas->setPixel(m_canvasDepthIndex,i,j, rgb, rgb, From 4226d57801e25ded91a6c7c6b3b16798d1a3edac Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 18 Nov 2017 15:19:27 -0800 Subject: [PATCH 12/30] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 81316647b..ae8e595b3 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.6.7', + version='1.6.8', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From c88132b80f59d1e06b527b6bc56a2b7e6f790bbc Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 18 Nov 2017 17:07:27 -0800 Subject: [PATCH 13/30] fix issue in pybullet_envs.agents bump up to pybullet 1.6.9 --- examples/pybullet/gym/pybullet_envs/agents/__init__.py | 2 +- examples/pybullet/gym/pybullet_envs/agents/configs.py | 9 +++++++++ setup.py | 2 +- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/agents/__init__.py b/examples/pybullet/gym/pybullet_envs/agents/__init__.py index 9e2c9f092..c29ce33fe 100644 --- a/examples/pybullet/gym/pybullet_envs/agents/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/agents/__init__.py @@ -20,4 +20,4 @@ from __future__ import print_function from . import train_ppo from . import utility -from . import visualize +from . import visualize_ppo diff --git a/examples/pybullet/gym/pybullet_envs/agents/configs.py b/examples/pybullet/gym/pybullet_envs/agents/configs.py index d613c29cb..a16fb1a62 100644 --- a/examples/pybullet/gym/pybullet_envs/agents/configs.py +++ b/examples/pybullet/gym/pybullet_envs/agents/configs.py @@ -119,6 +119,15 @@ def pybullet_racecar(): return locals() +def pybullet_humanoid(): + locals().update(default()) + randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer()) + env = 'HumanoidBulletEnv-v0' + max_length = 1000 + steps = 3e7 # 30M + return locals() + + def pybullet_minitaur(): """Configuration specific to minitaur_gym_env.MinitaurBulletEnv class.""" locals().update(default()) diff --git a/setup.py b/setup.py index ae8e595b3..a3ad02b43 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.6.8', + version='1.6.9', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 88289c032ef7c6721df61e891d00682cb76b7a13 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 18 Nov 2017 17:20:21 -0800 Subject: [PATCH 14/30] fix canvas issues from previous commit --- examples/RenderingExamples/RaytracerSetup.cpp | 2 +- examples/RenderingExamples/TimeSeriesCanvas.cpp | 2 +- examples/RenderingExamples/TinyVRGui.cpp | 2 +- examples/SharedMemory/PhysicsClientExample.cpp | 13 +++++++------ examples/SharedMemory/PhysicsServerExample.cpp | 2 +- 5 files changed, 11 insertions(+), 10 deletions(-) diff --git a/examples/RenderingExamples/RaytracerSetup.cpp b/examples/RenderingExamples/RaytracerSetup.cpp index 28d08fc9f..550451459 100644 --- a/examples/RenderingExamples/RaytracerSetup.cpp +++ b/examples/RenderingExamples/RaytracerSetup.cpp @@ -142,7 +142,7 @@ void RaytracerPhysicsSetup::initPhysics() if (m_internalData->m_canvas) { - m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("raytracer",m_internalData->m_width,m_internalData->m_height); + m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("raytracer",m_internalData->m_width,m_internalData->m_height, 15,55); for (int i=0;im_width;i++) { for (int j=0;jm_height;j++) diff --git a/examples/RenderingExamples/TimeSeriesCanvas.cpp b/examples/RenderingExamples/TimeSeriesCanvas.cpp index 096579839..60140b10e 100644 --- a/examples/RenderingExamples/TimeSeriesCanvas.cpp +++ b/examples/RenderingExamples/TimeSeriesCanvas.cpp @@ -78,7 +78,7 @@ TimeSeriesCanvas::TimeSeriesCanvas(struct Common2dCanvasInterface* canvasInterfa if (canvasInterface) { - m_internalData->m_canvasIndex = m_internalData->m_canvasInterface->createCanvas(windowTitle,m_internalData->m_width,m_internalData->m_height); + m_internalData->m_canvasIndex = m_internalData->m_canvasInterface->createCanvas(windowTitle,m_internalData->m_width,m_internalData->m_height,20,50); } } diff --git a/examples/RenderingExamples/TinyVRGui.cpp b/examples/RenderingExamples/TinyVRGui.cpp index 368606bf0..0b125dd33 100644 --- a/examples/RenderingExamples/TinyVRGui.cpp +++ b/examples/RenderingExamples/TinyVRGui.cpp @@ -28,7 +28,7 @@ struct TestCanvasInterface2 : public Common2dCanvasInterface virtual ~TestCanvasInterface2() {} - virtual int createCanvas(const char* canvasName, int width, int height) + virtual int createCanvas(const char* canvasName, int width, int height,int posX,int posY) { return 0; } diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index bdc65dac2..86aaa1a8a 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -22,8 +22,9 @@ struct MyMotorInfo2 int m_qIndex; }; -static int camVisualizerWidth = 320;//1024/3; -static int camVisualizerHeight = 240;//768/3; + +static int camVisualizerWidth = 228;//1024/3; +static int camVisualizerHeight = 192;//768/3; enum CustomCommands { @@ -758,10 +759,10 @@ void PhysicsClientExample::initPhysics() m_canvas = m_guiHelper->get2dCanvasInterface(); if (m_canvas) { - - m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight); - m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight); - m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight); + m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight, 8,55); + m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight,8,75+camVisualizerHeight); + m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight,8,95+camVisualizerHeight*2); + for (int i=0;i Date: Mon, 20 Nov 2017 11:05:31 -0800 Subject: [PATCH 15/30] reduce MAX_SDF_BODIES to 512, it causes stack overflow failures in some internal tests --- examples/SharedMemory/SharedMemoryPublic.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index fc8fa9c7c..b3e1b779f 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -355,7 +355,7 @@ enum b3VREventType #define MAX_KEYBOARD_EVENTS 256 #define MAX_MOUSE_EVENTS 256 -#define MAX_SDF_BODIES 1024 +#define MAX_SDF_BODIES 512 enum b3VRButtonInfo From 3f60be59ad0ea16dc02e98f53098c5d6f90dd0f8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 20 Nov 2017 12:35:36 -0800 Subject: [PATCH 16/30] bump up pybullet version to 1.7.0 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index a3ad02b43..fd26597bb 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.6.9', + version='1.7.0', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From ab843b26f06a3f91ebd13d11f1b3c134c55edd66 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 21 Nov 2017 17:05:28 -0800 Subject: [PATCH 17/30] expose a maximum velocity due to the joint motor in position control. see also pybullet/examples/motorMaxVelocity.py this fixes issue 1444 --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../SharedMemory/PhysicsServerCommandProcessor.cpp | 4 ++++ examples/SharedMemory/SharedMemoryCommands.h | 4 +++- examples/pybullet/examples/motorMaxVelocity.py | 12 ++++++++++++ examples/pybullet/pybullet.c | 11 ++++++++--- 6 files changed, 42 insertions(+), 4 deletions(-) create mode 100644 examples/pybullet/examples/motorMaxVelocity.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7b685974f..36e5d4330 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -606,6 +606,19 @@ B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, return 0; } +B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM)) + { + command->m_sendDesiredStateCommandArgument.m_rhsClamp[dofIndex] = maximumVelocity; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_RHS_CLAMP; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_RHS_CLAMP; + } + return 0; +} + B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f3bef30e5..63bc958cf 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -367,6 +367,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsC B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity); + ///Only use when controlMode is CONTROL_MODE_VELOCITY B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b96177ddc..5c1fafb71 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4531,6 +4531,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct if (motor) { + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex]&SIM_DESIRED_STATE_HAS_RHS_CLAMP)!=0) + { + motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]); + } bool hasDesiredPosOrVel = false; btScalar kp = 0.f; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 388a472d7..c16733e45 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -358,10 +358,11 @@ struct SendDesiredStateArgs { int m_bodyUniqueId; int m_controlMode; - + //PD parameters in case m_controlMode == CONTROL_MODE_POSITION_VELOCITY_PD double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + double m_rhsClamp[MAX_DEGREE_OF_FREEDOM]; int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM]; @@ -389,6 +390,7 @@ enum EnumSimDesiredStateUpdateFlags SIM_DESIRED_STATE_HAS_KD=4, SIM_DESIRED_STATE_HAS_KP=8, SIM_DESIRED_STATE_HAS_MAX_FORCE=16, + SIM_DESIRED_STATE_HAS_RHS_CLAMP=32 }; diff --git a/examples/pybullet/examples/motorMaxVelocity.py b/examples/pybullet/examples/motorMaxVelocity.py new file mode 100644 index 000000000..fad9e2fd4 --- /dev/null +++ b/examples/pybullet/examples/motorMaxVelocity.py @@ -0,0 +1,12 @@ +import pybullet as p +import time + +p.connect(p.GUI) +cartpole=p.loadURDF("cartpole.urdf") +p.setRealTimeSimulation(1) +p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5) +while (1): + p.setGravity(0,0,-10) + js = p.getJointState(cartpole,1) + print("position=",js[0],"velocity=",js[1]) + time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b11fcdc25..ade0ec1c8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1748,12 +1748,13 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, double force = 100000.0; double kp = 0.1; double kd = 1.0; + double maxVelocity = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode, - &targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|ddddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode, + &targetPosition, &targetVelocity, &force, &kp, &kd, &maxVelocity, &physicsClientId)) { //backward compatibility, bodyIndex -> bodyUniqueId, don't need to update this function: people have to migrate to bodyUniqueId static char* kwlist2[] = {"bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; @@ -1816,6 +1817,10 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, case CONTROL_MODE_POSITION_VELOCITY_PD: { + if (maxVelocity>0) + { + b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity); + } b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); b3JointControlSetKp(commandHandle, info.m_uIndex, kp); From 76772fd3014cea1fd626270bb9284e19ee49968c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 22 Nov 2017 15:30:56 -0800 Subject: [PATCH 18/30] reduce stack usage --- examples/SharedMemory/PhysicsDirect.cpp | 38 ++++++++++++++----------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index c747ff2fd..6dbac65db 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -29,6 +29,9 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_serverDNA; SharedMemoryCommand m_command; SharedMemoryStatus m_serverStatus; + + SharedMemoryCommand m_tmpInfoRequestCommand; + SharedMemoryStatus m_tmpInfoStatus; bool m_hasStatus; bool m_verboseOutput; @@ -79,6 +82,9 @@ struct PhysicsDirectInternalData PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) { + int sz = sizeof(SharedMemoryCommand); + int sz2 = sizeof(SharedMemoryStatus); + m_data = new PhysicsDirectInternalData; m_data->m_commandProcessor = physSdk; m_data->m_ownsCommandProcessor = passSdkOwnership; @@ -814,12 +820,12 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd for (int i=0;im_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + m_data->m_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; + m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; + m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); b3Clock clock; @@ -828,13 +834,13 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { - hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } if (hasStatus) { - int cid = infoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; - m_data->m_userConstraintInfoMap.insert(cid,infoStatus.m_userConstraintResultArgs); + int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,m_data->m_tmpInfoStatus.m_userConstraintResultArgs); } } @@ -842,11 +848,11 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd for (int i = 0; im_commandProcessor->processCommand(infoRequestCommand, infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; + m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); b3Clock clock; @@ -855,12 +861,12 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { - hasStatus = m_data->m_commandProcessor->receiveStatus(infoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } if (hasStatus) { - processBodyJointInfo(bodyUniqueId, infoStatus); + processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); } } break; From 29aa9cb78930413a84cc5bc962a212afb718d3af Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 22 Nov 2017 18:12:02 -0800 Subject: [PATCH 19/30] Bullet bump up to version 2.88 add preliminary support to import btMultiBody from a .bullet file (will help save/restore state) fix some Windows char/widechar issues --- .../BulletFileLoader/autogenerated/bullet.h | 24 + .../BulletFileLoader/btBulletFile.cpp | 5 + .../Serialize/BulletFileLoader/btBulletFile.h | 2 + .../BulletWorldImporter/CMakeLists.txt | 1 + .../btMultiBodyWorldImporter.cpp | 204 +++++++ .../btMultiBodyWorldImporter.h | 20 + Extras/Serialize/makesdna/makesdna.cpp | 2 + VERSION | 2 +- build3/premake4.lua | 4 +- .../CommonInterfaces/CommonMultiBodyBase.h | 13 + examples/OpenGLWindow/Win32Window.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 14 +- examples/SharedMemory/b3PluginManager.cpp | 2 +- examples/SharedMemory/premake4.lua | 3 + setup.py | 1 + .../CollisionDispatch/btCollisionWorld.cpp | 2 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 13 + .../Featherstone/btMultiBodyLinkCollider.h | 52 ++ src/LinearMath/btScalar.h | 2 +- src/LinearMath/btSerializer.cpp | 564 +++++++++--------- src/LinearMath/btSerializer.h | 3 +- src/LinearMath/btSerializer64.cpp | 564 +++++++++--------- src/clew/clew.c | 2 +- 23 files changed, 928 insertions(+), 573 deletions(-) create mode 100644 Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp create mode 100644 Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h diff --git a/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h b/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h index a4e4599be..2d85d0da4 100644 --- a/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h +++ b/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h @@ -110,6 +110,8 @@ typedef struct bInvalidHandle { class btMultiBodyLinkFloatData; class btMultiBodyDoubleData; class btMultiBodyFloatData; + class btMultiBodyLinkColliderFloatData; + class btMultiBodyLinkColliderDoubleData; // -------------------------------------------------- // class PointerArray { @@ -1440,5 +1442,27 @@ typedef struct bInvalidHandle { }; +// -------------------------------------------------- // + class btMultiBodyLinkColliderFloatData + { + public: + btCollisionObjectFloatData m_colObjData; + void *m_multiBody; + int m_link; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class btMultiBodyLinkColliderDoubleData + { + public: + btCollisionObjectDoubleData m_colObjData; + void *m_multiBody; + int m_link; + char m_padding[4]; + }; + + } #endif//__BULLET_H__ \ No newline at end of file diff --git a/Extras/Serialize/BulletFileLoader/btBulletFile.cpp b/Extras/Serialize/BulletFileLoader/btBulletFile.cpp index aa1c383fd..930027092 100644 --- a/Extras/Serialize/BulletFileLoader/btBulletFile.cpp +++ b/Extras/Serialize/BulletFileLoader/btBulletFile.cpp @@ -181,6 +181,11 @@ void btBulletFile::parseData() m_multiBodies.push_back((bStructHandle*)id); } + if (dataChunk.code == BT_MB_LINKCOLLIDER_CODE) + { + m_multiBodyLinkColliders.push_back((bStructHandle*)id); + } + if (dataChunk.code == BT_SOFTBODY_CODE) { m_softBodies.push_back((bStructHandle*) id); diff --git a/Extras/Serialize/BulletFileLoader/btBulletFile.h b/Extras/Serialize/BulletFileLoader/btBulletFile.h index 2296400a3..6433e436b 100644 --- a/Extras/Serialize/BulletFileLoader/btBulletFile.h +++ b/Extras/Serialize/BulletFileLoader/btBulletFile.h @@ -41,6 +41,8 @@ namespace bParse { btAlignedObjectArray m_multiBodies; + btAlignedObjectArray m_multiBodyLinkColliders; + btAlignedObjectArray m_softBodies; btAlignedObjectArray m_rigidBodies; diff --git a/Extras/Serialize/BulletWorldImporter/CMakeLists.txt b/Extras/Serialize/BulletWorldImporter/CMakeLists.txt index 3e6f00f10..b56b39a89 100644 --- a/Extras/Serialize/BulletWorldImporter/CMakeLists.txt +++ b/Extras/Serialize/BulletWorldImporter/CMakeLists.txt @@ -5,6 +5,7 @@ INCLUDE_DIRECTORIES( ADD_LIBRARY( BulletWorldImporter +btMultiBodyWorldImporter.cpp btBulletWorldImporter.cpp btBulletWorldImporter.h btWorldImporter.cpp diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp new file mode 100644 index 000000000..0473a4a3e --- /dev/null +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -0,0 +1,204 @@ +#include "btMultiBodyWorldImporter.h" + +#include "LinearMath/btSerializer.h" +#include "../BulletFileLoader/btBulletFile.h" +#include "btBulletWorldImporter.h" +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultibodyLinkCollider.h" + +struct btMultiBodyWorldImporterInternalData +{ + btMultiBodyDynamicsWorld* m_mbDynamicsWorld; + btHashMap m_mbMap; +}; + +btMultiBodyWorldImporter::btMultiBodyWorldImporter(btMultiBodyDynamicsWorld* world) + :btBulletWorldImporter(world) +{ + m_data = new btMultiBodyWorldImporterInternalData; + m_data->m_mbDynamicsWorld = world; +} + + +btMultiBodyWorldImporter::~btMultiBodyWorldImporter() +{ + delete m_data; +} + +void btMultiBodyWorldImporter::deleteAllData() +{ + btBulletWorldImporter::deleteAllData(); +} + + + +bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2) +{ + bool result = btBulletWorldImporter::convertAllObjects(bulletFile2); + + + //convert all multibodies + for (int i=0;im_multiBodies.size();i++) + { + + if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) + { + btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*) bulletFile2->m_multiBodies[i]; + bool isFixedBase = mbd->m_baseMass==0; + bool canSleep = false; + btVector3 baseInertia; + baseInertia.deSerializeDouble(mbd->m_baseInertia); + btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep); + mb->setHasSelfCollision(false); + + m_data->m_mbMap.insert(mbd,mb); + for (int i=0;im_numLinks;i++) + { + btVector3 localInertiaDiagonal; + localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia); + btQuaternion parentRotToThis; + parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis); + btVector3 parentComToThisPivotOffset; + parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset); + btVector3 thisPivotToThisComOffset; + thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset); + + switch (mbd->m_links[i].m_jointType) + { + case btMultibodyLink::eFixed: + { + + + mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex, + parentRotToThis, parentComToThisPivotOffset,thisPivotToThisComOffset); + //search for the collider + //mbd->m_links[i].m_linkCollider + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 jointAxis; + jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]); + bool disableParentCollision = true;//todo + mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex, + parentRotToThis,jointAxis, parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision); + break; + } + case btMultibodyLink::eRevolute: + { + btVector3 jointAxis; + jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]); + bool disableParentCollision = true;//todo + mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex, + parentRotToThis, jointAxis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision); + break; + } + case btMultibodyLink::eSpherical: + { + btAssert(0); + bool disableParentCollision = true;//todo + mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex, + parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision); + break; + } + case btMultibodyLink::ePlanar: + { + btAssert(0); + break; + } + default: + { + btAssert(0); + } + } + } + } + } + + //convert all multibody link colliders + for (int i=0;im_multiBodyLinkColliders.size();i++) + { + if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) + { + btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*) bulletFile2->m_multiBodyLinkColliders[i]; + + btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody]; + if (ptr) + { + btMultiBody* multiBody = *ptr; + + + btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + if (shape) + { + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link); + col->setCollisionShape( shape ); + + //btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name); + col->setFriction(btScalar(mblcd->m_colObjData.m_friction)); + col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution)); + //m_bodyMap.insert(colObjData,body); + if (mblcd->m_link==-1) + { + multiBody->setBaseCollider(col); + } else + { + multiBody->getLink(mblcd->m_link).m_collider = col; + } + int mbLinkIndex = mblcd->m_link; + + bool isDynamic = (mbLinkIndex<0 && multiBody->hasFixedBase())? false : true; + int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + #if 0 + int colGroup=0, colMask=0; + int collisionFlags = mblcd->m_colObjData.m_collisionFlags; + if (collisionFlags & URDF_HAS_COLLISION_GROUP) + { + collisionFilterGroup = colGroup; + } + if (collisionFlags & URDF_HAS_COLLISION_MASK) + { + collisionFilterMask = colMask; + } + #endif + m_data->m_mbDynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); + } + + } else + { + printf("error: no shape found\n"); + } +#if 0 + //base and fixed? -> static, otherwise flag as dynamic + + world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); +#endif + } + + } + } + + for (int i=0;im_mbMap.size();i++) + { + btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i); + if (ptr) + { + btMultiBody* mb = *ptr; + mb->finalizeMultiDof(); + + m_data->m_mbDynamicsWorld->addMultiBody(mb); + } + } + return result; +} \ No newline at end of file diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h new file mode 100644 index 000000000..eb47ecda2 --- /dev/null +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h @@ -0,0 +1,20 @@ +#ifndef BT_MULTIBODY_WORLD_IMPORTER_H +#define BT_MULTIBODY_WORLD_IMPORTER_H + +#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" + +class btMultiBodyWorldImporter : public btBulletWorldImporter +{ + struct btMultiBodyWorldImporterInternalData* m_data; + +public: + + btMultiBodyWorldImporter(class btMultiBodyDynamicsWorld* world); + virtual ~btMultiBodyWorldImporter(); + + virtual bool convertAllObjects( bParse::btBulletFile* bulletFile2); + + virtual void deleteAllData(); +}; + +#endif //BT_MULTIBODY_WORLD_IMPORTER_H diff --git a/Extras/Serialize/makesdna/makesdna.cpp b/Extras/Serialize/makesdna/makesdna.cpp index 105167da6..66b2467b6 100644 --- a/Extras/Serialize/makesdna/makesdna.cpp +++ b/Extras/Serialize/makesdna/makesdna.cpp @@ -149,6 +149,7 @@ typedef unsigned __int64 uint64_t; #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletSoftBody/btSoftBodyData.h" #include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #ifdef HAVE_CONFIG_H #include @@ -198,6 +199,7 @@ char *includefiles[] = { "../../../src/BulletSoftBody/btSoftBodyData.h", "../../../src/BulletDynamics/Featherstone/btMultiBody.h", + "../../../src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h", // empty string to indicate end of includefiles "" }; diff --git a/VERSION b/VERSION index 3274853c8..663ae72bc 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.87 +2.88 diff --git a/build3/premake4.lua b/build3/premake4.lua index c4ef1e4e8..6109f6729 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -284,9 +284,7 @@ end _OPTIONS["glfw_lib_name"] = default_glfw_lib_name end - if (_OPTIONS["enable_static_vr_plugin"]) then - defines("STATIC_LINK_VR_PLUGIN") - end + newoption { diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 54db4598d..7e3129477 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -233,6 +233,19 @@ struct CommonMultiBodyBase : public CommonExampleInterface virtual bool keyboardCallback(int key, int state) { + if ((key==B3G_F3) && state && m_dynamicsWorld) + { + btDefaultSerializer* serializer = new btDefaultSerializer(); + m_dynamicsWorld->serialize(serializer); + + FILE* file = fopen("testFile.bullet","wb"); + fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file); + fclose(file); + //b3Printf("btDefaultSerializer wrote testFile.bullet"); + delete serializer; + return true; + + } return false;//don't handle this key } diff --git a/examples/OpenGLWindow/Win32Window.cpp b/examples/OpenGLWindow/Win32Window.cpp index 78d590cc2..95149eb62 100644 --- a/examples/OpenGLWindow/Win32Window.cpp +++ b/examples/OpenGLWindow/Win32Window.cpp @@ -440,7 +440,7 @@ void Win32Window::setWindowTitle(const char* titleChar) { #ifdef _WIN64 - SetWindowText(m_data->m_hWnd, titleChar); + SetWindowTextA(m_data->m_hWnd, titleChar); #else DWORD dwResult; SendMessageTimeout(m_data->m_hWnd, WM_SETTEXT, 0, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5c1fafb71..45ccf3196 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -29,7 +29,7 @@ #include "LinearMath/btTransform.h" #include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h" #include "../Importers/ImportObjDemo/LoadMeshFromObj.h" -#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" +#include "../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "LinearMath/btSerializer.h" #include "Bullet3Common/b3Logging.h" @@ -1496,7 +1496,7 @@ struct PhysicsServerCommandProcessorInternalData b3AlignedObjectArray m_saveWorldBodyData; - btAlignedObjectArray m_worldImporters; + btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; btAlignedObjectArray m_strings; @@ -3458,7 +3458,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str bool hasStatus = true; serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); btCollisionShape* shape = 0; b3AlignedObjectArray urdfCollisionObjects; @@ -6505,7 +6505,7 @@ bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct S shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; } - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); btCollisionShape* shape = 0; @@ -8092,7 +8092,8 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_BULLET_LOADING_FAILED; - btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); + //btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" }; int numPrefixes = sizeof(prefix) / sizeof(const char*); @@ -8120,8 +8121,6 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared bool ok = importer->loadFile(relativeFileName); if (ok) { - - int numRb = importer->getNumRigidBodies(); serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; @@ -8213,6 +8212,7 @@ bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct Shared fclose(f); serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED; delete ser; + return hasStatus; } serverCmd.m_type = CMD_BULLET_SAVING_FAILED; return hasStatus; diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 7ea16c33a..afd352d41 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -13,7 +13,7 @@ typedef HMODULE B3_DYNLIB_HANDLE; - #define B3_DYNLIB_OPEN LoadLibrary + #define B3_DYNLIB_OPEN LoadLibraryA #define B3_DYNLIB_CLOSE FreeLibrary #define B3_DYNLIB_IMPORT GetProcAddress #else diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index add34d2fc..eca0ba160 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -111,6 +111,7 @@ files { } if (_OPTIONS["enable_static_vr_plugin"]) then + defines("STATIC_LINK_VR_PLUGIN") files {"plugins/vrSyncPlugin/vrSyncPlugin.cpp"} end @@ -208,6 +209,7 @@ files { "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", } if (_OPTIONS["enable_static_vr_plugin"]) then + defines("STATIC_LINK_VR_PLUGIN") files {"plugins/vrSyncPlugin/vrSyncPlugin.cpp"} end @@ -363,6 +365,7 @@ if os.is("Windows") then "../ThirdPartyLibs/openvr/samples/shared/Vectors.h", } if (_OPTIONS["enable_static_vr_plugin"]) then + defines("STATIC_LINK_VR_PLUGIN") files {"plugins/vrSyncPlugin/vrSyncPlugin.cpp"} end diff --git a/setup.py b/setup.py index fd26597bb..f326c8f85 100644 --- a/setup.py +++ b/setup.py @@ -274,6 +274,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["Extras/Serialize/BulletFileLoader/bDNA.cpp"]\ +["Extras/Serialize/BulletFileLoader/bFile.cpp"]\ +["Extras/Serialize/BulletFileLoader/btBulletFile.cpp"]\ ++["Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp"]\ +["Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp"]\ +["Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp"]\ +["Extras/InverseDynamics/CloneTreeCreator.cpp"]\ diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index c3e912fdc..59eb8dbe0 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1646,7 +1646,7 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) for (i=0;igetInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) { colObj->serializeSingleObject(serializer); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 5d2eb7e12..d50267b47 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -990,4 +990,17 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } + //serialize all multibody links (collision objects) + for (i=0;igetInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_MB_LINKCOLLIDER_CODE,colObj); + } + } + } \ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 671e15d31..4823b8c2e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -19,6 +19,16 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "btMultiBody.h" +#include "LinearMath/btSerializer.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData +#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData" +#else +#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData +#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData" +#endif + class btMultiBodyLinkCollider : public btCollisionObject { @@ -119,7 +129,49 @@ public: } return true; } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + }; + +struct btMultiBodyLinkColliderFloatData +{ + btCollisionObjectFloatData m_colObjData; + void *m_multiBody; + int m_link; + char m_padding[4]; +}; + +struct btMultiBodyLinkColliderDoubleData +{ + btCollisionObjectDoubleData m_colObjData; + void *m_multiBody; + int m_link; + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const +{ + return sizeof(btMultiBodyLinkColliderData); +} + +SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer; + btCollisionObject::serialize(&dataOut->m_colObjData,serializer); + + dataOut->m_link = this->m_link; + dataOut->m_multiBody = serializer->getUniquePointer(m_multiBody); + + // Fill padding with zeros to appease msan. + memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); + + return btMultiBodyLinkColliderDataName; +} + #endif //BT_FEATHERSTONE_LINK_COLLIDER_H diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index bffb2ce27..24e8454c1 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -25,7 +25,7 @@ subject to the following restrictions: #include /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 287 +#define BT_BULLET_VERSION 288 inline int btGetVersion() { diff --git a/src/LinearMath/btSerializer.cpp b/src/LinearMath/btSerializer.cpp index fcd2255ad..9635b59d7 100644 --- a/src/LinearMath/btSerializer.cpp +++ b/src/LinearMath/btSerializer.cpp @@ -1,5 +1,5 @@ char sBulletDNAstr[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-124),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-121),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), @@ -316,284 +316,292 @@ char(110),char(103),char(80),char(116),char(114),char(0),char(109),char(95),char char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97), char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(78), char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0), -char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115), -char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103), -char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0), -char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116), -char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97), -char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98), -char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68), -char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114), -char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111), -char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117), -char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105), -char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), -char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122), -char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110), -char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97), -char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114), -char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116), -char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105), -char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110), -char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110), -char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110), -char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116), -char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), -char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), -char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114), -char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115), -char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111), -char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70), -char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84), -char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112), -char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115), -char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111), -char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), -char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80), -char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117), -char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116), +char(109),char(95),char(99),char(111),char(108),char(79),char(98),char(106),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(109),char(117),char(108),char(116), +char(105),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69), +char(97),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116), +char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111), +char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100), +char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115), +char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98), +char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111), +char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101), +char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98), +char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118), +char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110), +char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83), +char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65), +char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101), +char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100), +char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100), +char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100), +char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114), +char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77), +char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105), +char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97), +char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103), +char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), +char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67), +char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), +char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109), +char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117), +char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79), +char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), +char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114), +char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97), +char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117), +char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), +char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100), +char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103), +char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50), +char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116), char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54), -char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114), -char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112), -char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98), -char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114), -char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101), -char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116), -char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83), -char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102), -char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102), -char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), -char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), -char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66), -char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66), -char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105), -char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108), -char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0), -char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0), -char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0), -char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0), -char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0), -char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-32),char(1),char(8),char(1),char(-104),char(0),char(88),char(0),char(-72),char(0), -char(104),char(0),char(-16),char(1),char(-80),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0), -char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2), -char(-124),char(2),char(-76),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0), -char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),char(-76),char(1),char(-16),char(2),char(-120),char(1),char(-64),char(0), -char(100),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0), -char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0), -char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0), -char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0), -char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0), -char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0), -char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0), -char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0), -char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0), -char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), -char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0), -char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0), -char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0), -char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0), -char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0), -char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), -char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0), -char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0), -char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0), -char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0), -char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0), -char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0), -char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0), -char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0), -char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0), -char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0), -char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0), -char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0), -char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0), -char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0), -char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), -char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0), -char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0), -char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0), -char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0), -char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0), -char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0), -char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0), -char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0), -char(49),char(0),char(27),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0), -char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0), -char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0), -char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(8),char(0),char(117),char(0),char(8),char(0),char(118),char(0), +char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105), +char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), +char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67), +char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99), +char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101), +char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83), +char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67), +char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50), +char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103), +char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101), +char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), +char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101), +char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68), +char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97), +char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111), +char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66), +char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103), +char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), +char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121), +char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117), +char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74), +char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121), +char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108), +char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111), +char(108),char(108),char(105),char(100),char(101),char(114),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117), +char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0), +char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0), +char(8),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0), +char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),char(20),char(0),char(64),char(0), +char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),char(76),char(0),char(76),char(0), +char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-32),char(1),char(8),char(1),char(-104),char(0), +char(88),char(0),char(-72),char(0),char(104),char(0),char(-16),char(1),char(-80),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0), +char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1), +char(92),char(1),char(116),char(2),char(-124),char(2),char(-76),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0), +char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),char(-76),char(1),char(-16),char(2), +char(-120),char(1),char(-64),char(0),char(100),char(0),char(20),char(1),char(-20),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(86),char(0),char(0),char(0), +char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0), +char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0), +char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0), +char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0), +char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0), +char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0), +char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0), +char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), +char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), +char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0), +char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), +char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), +char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), +char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), +char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0), +char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), +char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0), +char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0), +char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0), +char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0), +char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), +char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0), +char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0), +char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0), +char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), +char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), +char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0), +char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0), +char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0), +char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0), +char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), +char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), +char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0), +char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0), +char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0), +char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), +char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), +char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(27),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), +char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0), +char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), +char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), +char(8),char(0),char(117),char(0),char(8),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), +char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0), +char(50),char(0),char(27),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0), +char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0),char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0), +char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0), +char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),char(7),char(0),char(117),char(0),char(7),char(0),char(118),char(0), char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0), -char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(27),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0), -char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), -char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(116),char(0),char(7),char(0),char(117),char(0),char(7),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0), -char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(126),char(0),char(8),char(0),char(127),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(-128),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0), -char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),char(0),char(-122),char(0),char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), -char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0), -char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0), -char(7),char(0),char(126),char(0),char(7),char(0),char(127),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-128),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0), -char(7),char(0),char(-122),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), -char(7),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0), -char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0),char(51),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0), -char(54),char(0),char(2),char(0),char(52),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0),char(55),char(0),char(21),char(0),char(50),char(0),char(-109),char(0), -char(17),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0),char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0), -char(13),char(0),char(-110),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0),char(13),char(0),char(-101),char(0),char(13),char(0),char(-100),char(0), -char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0),char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0), -char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(7),char(0),char(-92),char(0),char(7),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), -char(56),char(0),char(22),char(0),char(49),char(0),char(-109),char(0),char(18),char(0),char(-108),char(0),char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0), -char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),char(14),char(0),char(-110),char(0),char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0), -char(14),char(0),char(-101),char(0),char(14),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0), -char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0),char(8),char(0),char(-93),char(0),char(8),char(0),char(-92),char(0), -char(8),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(0),char(0),char(37),char(0),char(57),char(0),char(2),char(0),char(4),char(0),char(-89),char(0), -char(4),char(0),char(-88),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-87),char(0),char(55),char(0),char(-86),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0), -char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0), -char(59),char(0),char(13),char(0),char(60),char(0),char(-87),char(0),char(60),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0), -char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0), -char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(61),char(0),char(14),char(0), -char(56),char(0),char(-87),char(0),char(56),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0), -char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(8),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0), -char(4),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(0),char(0),char(-75),char(0),char(62),char(0),char(3),char(0), -char(59),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(63),char(0),char(3),char(0),char(61),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(64),char(0),char(3),char(0),char(59),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0), -char(14),char(0),char(-72),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0), -char(66),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0), -char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0), -char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(67),char(0),char(14),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0), -char(4),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0), -char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0),char(0),char(0),char(-59),char(0),char(68),char(0),char(10),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0), -char(8),char(0),char(-56),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(127),char(0), -char(69),char(0),char(11),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(7),char(0),char(-58),char(0), -char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(127),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), -char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(71),char(0),char(9),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0), -char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(72),char(0),char(5),char(0),char(70),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(7),char(0),char(-47),char(0),char(7),char(0),char(-46),char(0),char(7),char(0),char(-45),char(0),char(73),char(0),char(5),char(0),char(71),char(0),char(-49),char(0), -char(4),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(8),char(0),char(-46),char(0),char(8),char(0),char(-45),char(0),char(74),char(0),char(41),char(0), +char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(126),char(0), +char(8),char(0),char(127),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(-128),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(-127),char(0), +char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0),char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),char(0),char(-122),char(0), +char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0), +char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0), +char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0),char(7),char(0),char(126),char(0),char(7),char(0),char(127),char(0),char(7),char(0),char(111),char(0), +char(7),char(0),char(-128),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0), +char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0),char(7),char(0),char(-122),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0), +char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),char(7),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0), +char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0), +char(51),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0),char(54),char(0),char(2),char(0),char(52),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0), +char(55),char(0),char(21),char(0),char(50),char(0),char(-109),char(0),char(17),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0), +char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0),char(13),char(0),char(-110),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0), +char(13),char(0),char(-101),char(0),char(13),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0), +char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0),char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(7),char(0),char(-92),char(0), +char(7),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(-109),char(0),char(18),char(0),char(-108),char(0), +char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0),char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),char(14),char(0),char(-110),char(0), +char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0),char(14),char(0),char(-101),char(0),char(14),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0), +char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0),char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0), +char(8),char(0),char(-93),char(0),char(8),char(0),char(-92),char(0),char(8),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(0),char(0),char(37),char(0), +char(57),char(0),char(2),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-87),char(0), +char(55),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0), +char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0), +char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-87),char(0),char(60),char(0),char(-86),char(0), +char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0), +char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0), +char(4),char(0),char(-76),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-87),char(0),char(56),char(0),char(-86),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), +char(8),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0), +char(0),char(0),char(-75),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0), +char(63),char(0),char(3),char(0),char(61),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(64),char(0),char(3),char(0), +char(59),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-74),char(0), +char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0), +char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0), +char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), +char(19),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0), +char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0), +char(7),char(0),char(-60),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), +char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0), +char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0), +char(0),char(0),char(-59),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), +char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0), +char(8),char(0),char(-60),char(0),char(8),char(0),char(127),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), +char(19),char(0),char(-70),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-62),char(0), +char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(7),char(0),char(127),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0), char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0), -char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0), -char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), -char(13),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0), -char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0),char(13),char(0),char(-28),char(0), -char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0),char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0), -char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0),char(13),char(0),char(-19),char(0),char(13),char(0),char(-18),char(0), -char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0), -char(4),char(0),char(-12),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0), -char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(14),char(0),char(-37),char(0), -char(14),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0), -char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(14),char(0),char(-53),char(0), -char(14),char(0),char(-52),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0),char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0), -char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0),char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0), -char(14),char(0),char(-19),char(0),char(14),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0), -char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(76),char(0),char(9),char(0),char(59),char(0),char(-74),char(0), -char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0), -char(7),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(77),char(0),char(9),char(0),char(61),char(0),char(-74),char(0), -char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0), -char(8),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(78),char(0),char(5),char(0),char(58),char(0),char(-74),char(0), -char(13),char(0),char(-11),char(0),char(13),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0), -char(61),char(0),char(-74),char(0),char(14),char(0),char(-11),char(0),char(14),char(0),char(-10),char(0),char(8),char(0),char(-9),char(0),char(80),char(0),char(4),char(0), -char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0), -char(80),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0), -char(13),char(0),char(0),char(1),char(7),char(0),char(-99),char(0),char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(53),char(0), -char(82),char(0),char(4),char(0),char(80),char(0),char(-5),char(0),char(4),char(0),char(3),char(1),char(7),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(83),char(0),char(4),char(0),char(13),char(0),char(0),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(6),char(1),char(7),char(0),char(7),char(1), -char(84),char(0),char(7),char(0),char(13),char(0),char(8),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(9),char(1),char(7),char(0),char(10),char(1), -char(7),char(0),char(11),char(1),char(7),char(0),char(12),char(1),char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(13),char(1), -char(13),char(0),char(11),char(1),char(13),char(0),char(14),char(1),char(60),char(0),char(15),char(1),char(4),char(0),char(16),char(1),char(7),char(0),char(12),char(1), -char(86),char(0),char(26),char(0),char(4),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(127),char(0),char(7),char(0),char(19),char(1), -char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), -char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(39),char(1), -char(4),char(0),char(40),char(1),char(4),char(0),char(120),char(0),char(87),char(0),char(12),char(0),char(17),char(0),char(41),char(1),char(17),char(0),char(42),char(1), -char(17),char(0),char(43),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(45),char(1),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(4),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0), -char(88),char(0),char(27),char(0),char(19),char(0),char(51),char(1),char(17),char(0),char(52),char(1),char(17),char(0),char(53),char(1),char(13),char(0),char(44),char(1), -char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(13),char(0),char(57),char(1),char(13),char(0),char(58),char(1), -char(4),char(0),char(59),char(1),char(7),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(7),char(0),char(68),char(1), -char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1), -char(4),char(0),char(74),char(1),char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(77),char(1), -char(9),char(0),char(78),char(1),char(13),char(0),char(79),char(1),char(7),char(0),char(80),char(1),char(7),char(0),char(-125),char(0),char(7),char(0),char(81),char(1), -char(4),char(0),char(82),char(1),char(13),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(85),char(1),char(4),char(0),char(86),char(1), -char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-109),char(0),char(87),char(0),char(87),char(1),char(80),char(0),char(88),char(1), -char(81),char(0),char(89),char(1),char(82),char(0),char(90),char(1),char(83),char(0),char(91),char(1),char(84),char(0),char(92),char(1),char(85),char(0),char(93),char(1), -char(88),char(0),char(94),char(1),char(89),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(97),char(1), -char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1), -char(86),char(0),char(103),char(1),char(91),char(0),char(24),char(0),char(16),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), -char(14),char(0),char(107),char(1),char(14),char(0),char(108),char(1),char(14),char(0),char(109),char(1),char(8),char(0),char(110),char(1),char(4),char(0),char(111),char(1), -char(4),char(0),char(86),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1),char(8),char(0),char(114),char(1),char(8),char(0),char(115),char(1), -char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(8),char(0),char(118),char(1),char(8),char(0),char(119),char(1),char(8),char(0),char(120),char(1), -char(8),char(0),char(121),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(49),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(92),char(0),char(24),char(0),char(15),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1), -char(13),char(0),char(107),char(1),char(13),char(0),char(108),char(1),char(13),char(0),char(109),char(1),char(4),char(0),char(112),char(1),char(7),char(0),char(110),char(1), -char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(7),char(0),char(114),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1), -char(4),char(0),char(113),char(1),char(7),char(0),char(117),char(1),char(7),char(0),char(118),char(1),char(7),char(0),char(119),char(1),char(7),char(0),char(120),char(1), -char(7),char(0),char(121),char(1),char(7),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(50),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(127),char(1),char(14),char(0),char(-128),char(1),char(8),char(0),char(-127),char(1), -char(0),char(0),char(-126),char(1),char(91),char(0),char(90),char(1),char(49),char(0),char(-125),char(1),char(0),char(0),char(126),char(1),char(4),char(0),char(97),char(1), -char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(-126),char(1),char(92),char(0),char(90),char(1),char(50),char(0),char(-125),char(1), -char(19),char(0),char(127),char(1),char(13),char(0),char(-128),char(1),char(7),char(0),char(-127),char(1),char(4),char(0),char(97),char(1),}; +char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(71),char(0),char(9),char(0), +char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0), +char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(72),char(0),char(5),char(0), +char(70),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(7),char(0),char(-46),char(0),char(7),char(0),char(-45),char(0), +char(73),char(0),char(5),char(0),char(71),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(8),char(0),char(-46),char(0), +char(8),char(0),char(-45),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0), +char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0), +char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0), +char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0),char(13),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0), +char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(13),char(0),char(-53),char(0), +char(13),char(0),char(-52),char(0),char(13),char(0),char(-28),char(0),char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0), +char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0),char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0), +char(13),char(0),char(-19),char(0),char(13),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0), +char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-74),char(0), +char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-44),char(0), +char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0), +char(14),char(0),char(-38),char(0),char(14),char(0),char(-37),char(0),char(14),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0), +char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0), +char(0),char(0),char(-59),char(0),char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0), +char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0),char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0), +char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0),char(14),char(0),char(-19),char(0),char(14),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0), +char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(76),char(0),char(9),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(7),char(0),char(-55),char(0), +char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0), +char(77),char(0),char(9),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(8),char(0),char(-55),char(0), +char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0),char(8),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0), +char(78),char(0),char(5),char(0),char(58),char(0),char(-74),char(0),char(13),char(0),char(-11),char(0),char(13),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0), +char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-74),char(0),char(14),char(0),char(-11),char(0),char(14),char(0),char(-10),char(0), +char(8),char(0),char(-9),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0), +char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0), +char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0),char(13),char(0),char(0),char(1),char(7),char(0),char(-99),char(0),char(7),char(0),char(1),char(1), +char(4),char(0),char(2),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-5),char(0),char(4),char(0),char(3),char(1), +char(7),char(0),char(4),char(1),char(4),char(0),char(5),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(0),char(1),char(80),char(0),char(-5),char(0), +char(4),char(0),char(6),char(1),char(7),char(0),char(7),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(8),char(1),char(80),char(0),char(-5),char(0), +char(4),char(0),char(9),char(1),char(7),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(7),char(0),char(12),char(1),char(4),char(0),char(53),char(0), +char(85),char(0),char(6),char(0),char(17),char(0),char(13),char(1),char(13),char(0),char(11),char(1),char(13),char(0),char(14),char(1),char(60),char(0),char(15),char(1), +char(4),char(0),char(16),char(1),char(7),char(0),char(12),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(17),char(1),char(7),char(0),char(18),char(1), +char(7),char(0),char(127),char(0),char(7),char(0),char(19),char(1),char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1), +char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1), +char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1), +char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(4),char(0),char(37),char(1), +char(4),char(0),char(38),char(1),char(4),char(0),char(39),char(1),char(4),char(0),char(40),char(1),char(4),char(0),char(120),char(0),char(87),char(0),char(12),char(0), +char(17),char(0),char(41),char(1),char(17),char(0),char(42),char(1),char(17),char(0),char(43),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(45),char(1), +char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1),char(4),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1), +char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(51),char(1),char(17),char(0),char(52),char(1), +char(17),char(0),char(53),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1), +char(13),char(0),char(57),char(1),char(13),char(0),char(58),char(1),char(4),char(0),char(59),char(1),char(7),char(0),char(60),char(1),char(4),char(0),char(61),char(1), +char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1),char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),char(4),char(0),char(66),char(1), +char(4),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1), +char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1),char(4),char(0),char(74),char(1),char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1), +char(89),char(0),char(12),char(0),char(9),char(0),char(77),char(1),char(9),char(0),char(78),char(1),char(13),char(0),char(79),char(1),char(7),char(0),char(80),char(1), +char(7),char(0),char(-125),char(0),char(7),char(0),char(81),char(1),char(4),char(0),char(82),char(1),char(13),char(0),char(83),char(1),char(4),char(0),char(84),char(1), +char(4),char(0),char(85),char(1),char(4),char(0),char(86),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-109),char(0), +char(87),char(0),char(87),char(1),char(80),char(0),char(88),char(1),char(81),char(0),char(89),char(1),char(82),char(0),char(90),char(1),char(83),char(0),char(91),char(1), +char(84),char(0),char(92),char(1),char(85),char(0),char(93),char(1),char(88),char(0),char(94),char(1),char(89),char(0),char(95),char(1),char(4),char(0),char(96),char(1), +char(4),char(0),char(62),char(1),char(4),char(0),char(97),char(1),char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1), +char(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1),char(86),char(0),char(103),char(1),char(91),char(0),char(24),char(0),char(16),char(0),char(104),char(1), +char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),char(14),char(0),char(107),char(1),char(14),char(0),char(108),char(1),char(14),char(0),char(109),char(1), +char(8),char(0),char(110),char(1),char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1), +char(8),char(0),char(114),char(1),char(8),char(0),char(115),char(1),char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(8),char(0),char(118),char(1), +char(8),char(0),char(119),char(1),char(8),char(0),char(120),char(1),char(8),char(0),char(121),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(123),char(1), +char(0),char(0),char(124),char(1),char(49),char(0),char(125),char(1),char(0),char(0),char(126),char(1),char(92),char(0),char(24),char(0),char(15),char(0),char(104),char(1), +char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(13),char(0),char(107),char(1),char(13),char(0),char(108),char(1),char(13),char(0),char(109),char(1), +char(4),char(0),char(112),char(1),char(7),char(0),char(110),char(1),char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(7),char(0),char(114),char(1), +char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(117),char(1),char(7),char(0),char(118),char(1), +char(7),char(0),char(119),char(1),char(7),char(0),char(120),char(1),char(7),char(0),char(121),char(1),char(7),char(0),char(122),char(1),char(0),char(0),char(123),char(1), +char(0),char(0),char(124),char(1),char(50),char(0),char(125),char(1),char(0),char(0),char(126),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(127),char(1), +char(14),char(0),char(-128),char(1),char(8),char(0),char(-127),char(1),char(0),char(0),char(-126),char(1),char(91),char(0),char(90),char(1),char(49),char(0),char(-125),char(1), +char(0),char(0),char(126),char(1),char(4),char(0),char(97),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(-126),char(1), +char(92),char(0),char(90),char(1),char(50),char(0),char(-125),char(1),char(19),char(0),char(127),char(1),char(13),char(0),char(-128),char(1),char(7),char(0),char(-127),char(1), +char(4),char(0),char(97),char(1),char(95),char(0),char(4),char(0),char(50),char(0),char(-124),char(1),char(9),char(0),char(-123),char(1),char(4),char(0),char(-122),char(1), +char(0),char(0),char(37),char(0),char(96),char(0),char(4),char(0),char(49),char(0),char(-124),char(1),char(9),char(0),char(-123),char(1),char(4),char(0),char(-122),char(1), +char(0),char(0),char(37),char(0),}; int sBulletDNAlen= sizeof(sBulletDNAstr); diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h index 89b4d7468..1fda2c665 100644 --- a/src/LinearMath/btSerializer.h +++ b/src/LinearMath/btSerializer.h @@ -115,6 +115,7 @@ public: #define BT_MULTIBODY_CODE BT_MAKE_ID('M','B','D','Y') +#define BT_MB_LINKCOLLIDER_CODE BT_MAKE_ID('M','B','L','C') #define BT_SOFTBODY_CODE BT_MAKE_ID('S','B','D','Y') #define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J') #define BT_RIGIDBODY_CODE BT_MAKE_ID('R','B','D','Y') @@ -505,7 +506,7 @@ public: buffer[9] = '2'; buffer[10] = '8'; - buffer[11] = '7'; + buffer[11] = '8'; } diff --git a/src/LinearMath/btSerializer64.cpp b/src/LinearMath/btSerializer64.cpp index 05f59202d..45c00e678 100644 --- a/src/LinearMath/btSerializer64.cpp +++ b/src/LinearMath/btSerializer64.cpp @@ -1,5 +1,5 @@ char sBulletDNAstr64[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-124),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-121),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), @@ -316,284 +316,292 @@ char(110),char(103),char(80),char(116),char(114),char(0),char(109),char(95),char char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97), char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(78), char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0), -char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115), -char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103), -char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0), -char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116), -char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97), -char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98), -char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68), -char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114), -char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111), -char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117), -char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105), -char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), -char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122), -char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110), -char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97), -char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114), -char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116), -char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105), -char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110), -char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110), -char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110), -char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116), -char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), -char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), -char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114), -char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115), -char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111), -char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70), -char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84), -char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112), -char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115), -char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111), -char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), -char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80), -char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117), -char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116), +char(109),char(95),char(99),char(111),char(108),char(79),char(98),char(106),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(109),char(117),char(108),char(116), +char(105),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69), +char(97),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116), +char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111), +char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100), +char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115), +char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98), +char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111), +char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101), +char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98), +char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118), +char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110), +char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83), +char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65), +char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101), +char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100), +char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100), +char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100), +char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114), +char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77), +char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105), +char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97), +char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103), +char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), +char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67), +char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), +char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109), +char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117), +char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79), +char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), +char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114), +char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97), +char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117), +char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), +char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100), +char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103), +char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50), +char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116), char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54), -char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114), -char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112), -char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98), -char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114), -char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101), -char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116), -char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83), -char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102), -char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102), -char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), -char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), -char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66), -char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66), -char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105), -char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108), -char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0), -char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0), -char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0), -char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0), -char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0), -char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-16),char(1),char(24),char(1),char(-104),char(0),char(88),char(0),char(-72),char(0), -char(104),char(0),char(0),char(2),char(-64),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0), -char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2), -char(-112),char(2),char(-64),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0), -char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),char(-24),char(1),char(0),char(3),char(-104),char(1),char(-48),char(0), -char(112),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0), -char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0), -char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0), -char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0), -char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0), -char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0), -char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0), -char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0), -char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0), -char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), -char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0), -char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0), -char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0), -char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0), -char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0), -char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), -char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0), -char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0), -char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0), -char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0), -char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0), -char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0), -char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0), -char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0), -char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0), -char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0), -char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0), -char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0), -char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0), -char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0), -char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), -char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0), -char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0), -char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0), -char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0), -char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0), -char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0), -char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0), -char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0), -char(49),char(0),char(27),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0), -char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0), -char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0), -char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(8),char(0),char(117),char(0),char(8),char(0),char(118),char(0), +char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), +char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105), +char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), +char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67), +char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99), +char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101), +char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83), +char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67), +char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50), +char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103), +char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101), +char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), +char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101), +char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68), +char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97), +char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111), +char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66), +char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103), +char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), +char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121), +char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117), +char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74), +char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121), +char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108), +char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111), +char(108),char(108),char(105),char(100),char(101),char(114),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117), +char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0), +char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0), +char(16),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0), +char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),char(20),char(0),char(72),char(0), +char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),char(96),char(0),char(80),char(0), +char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-16),char(1),char(24),char(1),char(-104),char(0), +char(88),char(0),char(-72),char(0),char(104),char(0),char(0),char(2),char(-64),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0), +char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1), +char(104),char(1),char(-128),char(2),char(-112),char(2),char(-64),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0), +char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),char(-24),char(1),char(0),char(3), +char(-104),char(1),char(-48),char(0),char(112),char(0),char(40),char(1),char(0),char(2),char(0),char(0),char(83),char(84),char(82),char(67),char(86),char(0),char(0),char(0), +char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0), +char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0), +char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0), +char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0), +char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0), +char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0), +char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0), +char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), +char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), +char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0), +char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), +char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), +char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), +char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), +char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0), +char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), +char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0), +char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0), +char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0), +char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0), +char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), +char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0), +char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0), +char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0), +char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), +char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), +char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0), +char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0), +char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0), +char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0), +char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), +char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), +char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0), +char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0), +char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0), +char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), +char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), +char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(27),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), +char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0), +char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), +char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), +char(8),char(0),char(117),char(0),char(8),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), +char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0), +char(50),char(0),char(27),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0), +char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0),char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0), +char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0), +char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),char(7),char(0),char(117),char(0),char(7),char(0),char(118),char(0), char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0), -char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(27),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0), -char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), -char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(116),char(0),char(7),char(0),char(117),char(0),char(7),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0), -char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(126),char(0),char(8),char(0),char(127),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(-128),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0), -char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),char(0),char(-122),char(0),char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), -char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0), -char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0), -char(7),char(0),char(126),char(0),char(7),char(0),char(127),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-128),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0), -char(7),char(0),char(-122),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), -char(7),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0), -char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0),char(51),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0), -char(54),char(0),char(2),char(0),char(52),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0),char(55),char(0),char(21),char(0),char(50),char(0),char(-109),char(0), -char(17),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0),char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0), -char(13),char(0),char(-110),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0),char(13),char(0),char(-101),char(0),char(13),char(0),char(-100),char(0), -char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0),char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0), -char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(7),char(0),char(-92),char(0),char(7),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), -char(56),char(0),char(22),char(0),char(49),char(0),char(-109),char(0),char(18),char(0),char(-108),char(0),char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0), -char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),char(14),char(0),char(-110),char(0),char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0), -char(14),char(0),char(-101),char(0),char(14),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0), -char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0),char(8),char(0),char(-93),char(0),char(8),char(0),char(-92),char(0), -char(8),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(0),char(0),char(37),char(0),char(57),char(0),char(2),char(0),char(4),char(0),char(-89),char(0), -char(4),char(0),char(-88),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-87),char(0),char(55),char(0),char(-86),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0), -char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0), -char(59),char(0),char(13),char(0),char(60),char(0),char(-87),char(0),char(60),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0), -char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0), -char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(61),char(0),char(14),char(0), -char(56),char(0),char(-87),char(0),char(56),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0), -char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(8),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0), -char(4),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(0),char(0),char(-75),char(0),char(62),char(0),char(3),char(0), -char(59),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(63),char(0),char(3),char(0),char(61),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(64),char(0),char(3),char(0),char(59),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0), -char(14),char(0),char(-72),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0), -char(66),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0), -char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0), -char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(67),char(0),char(14),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0), -char(4),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0), -char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0),char(0),char(0),char(-59),char(0),char(68),char(0),char(10),char(0), -char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0), -char(8),char(0),char(-56),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(127),char(0), -char(69),char(0),char(11),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(7),char(0),char(-58),char(0), -char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(127),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), -char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(71),char(0),char(9),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0), -char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0), -char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(72),char(0),char(5),char(0),char(70),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(7),char(0),char(-47),char(0),char(7),char(0),char(-46),char(0),char(7),char(0),char(-45),char(0),char(73),char(0),char(5),char(0),char(71),char(0),char(-49),char(0), -char(4),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(8),char(0),char(-46),char(0),char(8),char(0),char(-45),char(0),char(74),char(0),char(41),char(0), +char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(126),char(0), +char(8),char(0),char(127),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(-128),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(-127),char(0), +char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0),char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),char(0),char(-122),char(0), +char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0), +char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0), +char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0),char(7),char(0),char(126),char(0),char(7),char(0),char(127),char(0),char(7),char(0),char(111),char(0), +char(7),char(0),char(-128),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0), +char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0),char(7),char(0),char(-122),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0), +char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),char(7),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0), +char(4),char(0),char(-114),char(0),char(4),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0), +char(51),char(0),char(-111),char(0),char(14),char(0),char(-110),char(0),char(54),char(0),char(2),char(0),char(52),char(0),char(-111),char(0),char(13),char(0),char(-110),char(0), +char(55),char(0),char(21),char(0),char(50),char(0),char(-109),char(0),char(17),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0), +char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0),char(13),char(0),char(-110),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0), +char(13),char(0),char(-101),char(0),char(13),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0), +char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0),char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(7),char(0),char(-92),char(0), +char(7),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(-109),char(0),char(18),char(0),char(-108),char(0), +char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0),char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),char(14),char(0),char(-110),char(0), +char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0),char(14),char(0),char(-101),char(0),char(14),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0), +char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0),char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0), +char(8),char(0),char(-93),char(0),char(8),char(0),char(-92),char(0),char(8),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(0),char(0),char(37),char(0), +char(57),char(0),char(2),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-87),char(0), +char(55),char(0),char(-86),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0), +char(4),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0), +char(7),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-87),char(0),char(60),char(0),char(-86),char(0), +char(0),char(0),char(35),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0), +char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0), +char(4),char(0),char(-76),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-87),char(0),char(56),char(0),char(-86),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(4),char(0),char(-83),char(0),char(4),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), +char(8),char(0),char(-80),char(0),char(4),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0), +char(0),char(0),char(-75),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-74),char(0),char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0), +char(63),char(0),char(3),char(0),char(61),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(64),char(0),char(3),char(0), +char(59),char(0),char(-74),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-74),char(0), +char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0), +char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0), +char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), +char(19),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0), +char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0), +char(7),char(0),char(-60),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), +char(4),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(4),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0), +char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-60),char(0), +char(0),char(0),char(-59),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), +char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0), +char(8),char(0),char(-60),char(0),char(8),char(0),char(127),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0), +char(19),char(0),char(-70),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-62),char(0), +char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(7),char(0),char(127),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0), char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0), -char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0), -char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), -char(13),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0), -char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0),char(13),char(0),char(-28),char(0), -char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0),char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0), -char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0),char(13),char(0),char(-19),char(0),char(13),char(0),char(-18),char(0), -char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0), -char(4),char(0),char(-12),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0), -char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0), -char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(14),char(0),char(-37),char(0), -char(14),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0), -char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(14),char(0),char(-53),char(0), -char(14),char(0),char(-52),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0),char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0), -char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0),char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0), -char(14),char(0),char(-19),char(0),char(14),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0), -char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(76),char(0),char(9),char(0),char(59),char(0),char(-74),char(0), -char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0), -char(7),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(77),char(0),char(9),char(0),char(61),char(0),char(-74),char(0), -char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0), -char(8),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(78),char(0),char(5),char(0),char(58),char(0),char(-74),char(0), -char(13),char(0),char(-11),char(0),char(13),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0), -char(61),char(0),char(-74),char(0),char(14),char(0),char(-11),char(0),char(14),char(0),char(-10),char(0),char(8),char(0),char(-9),char(0),char(80),char(0),char(4),char(0), -char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0), -char(80),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0), -char(13),char(0),char(0),char(1),char(7),char(0),char(-99),char(0),char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(53),char(0), -char(82),char(0),char(4),char(0),char(80),char(0),char(-5),char(0),char(4),char(0),char(3),char(1),char(7),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(83),char(0),char(4),char(0),char(13),char(0),char(0),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(6),char(1),char(7),char(0),char(7),char(1), -char(84),char(0),char(7),char(0),char(13),char(0),char(8),char(1),char(80),char(0),char(-5),char(0),char(4),char(0),char(9),char(1),char(7),char(0),char(10),char(1), -char(7),char(0),char(11),char(1),char(7),char(0),char(12),char(1),char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(13),char(1), -char(13),char(0),char(11),char(1),char(13),char(0),char(14),char(1),char(60),char(0),char(15),char(1),char(4),char(0),char(16),char(1),char(7),char(0),char(12),char(1), -char(86),char(0),char(26),char(0),char(4),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(127),char(0),char(7),char(0),char(19),char(1), -char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), -char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(39),char(1), -char(4),char(0),char(40),char(1),char(4),char(0),char(120),char(0),char(87),char(0),char(12),char(0),char(17),char(0),char(41),char(1),char(17),char(0),char(42),char(1), -char(17),char(0),char(43),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(45),char(1),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(4),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0), -char(88),char(0),char(27),char(0),char(19),char(0),char(51),char(1),char(17),char(0),char(52),char(1),char(17),char(0),char(53),char(1),char(13),char(0),char(44),char(1), -char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(13),char(0),char(57),char(1),char(13),char(0),char(58),char(1), -char(4),char(0),char(59),char(1),char(7),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(7),char(0),char(68),char(1), -char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1), -char(4),char(0),char(74),char(1),char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(77),char(1), -char(9),char(0),char(78),char(1),char(13),char(0),char(79),char(1),char(7),char(0),char(80),char(1),char(7),char(0),char(-125),char(0),char(7),char(0),char(81),char(1), -char(4),char(0),char(82),char(1),char(13),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(85),char(1),char(4),char(0),char(86),char(1), -char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-109),char(0),char(87),char(0),char(87),char(1),char(80),char(0),char(88),char(1), -char(81),char(0),char(89),char(1),char(82),char(0),char(90),char(1),char(83),char(0),char(91),char(1),char(84),char(0),char(92),char(1),char(85),char(0),char(93),char(1), -char(88),char(0),char(94),char(1),char(89),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(97),char(1), -char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1), -char(86),char(0),char(103),char(1),char(91),char(0),char(24),char(0),char(16),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), -char(14),char(0),char(107),char(1),char(14),char(0),char(108),char(1),char(14),char(0),char(109),char(1),char(8),char(0),char(110),char(1),char(4),char(0),char(111),char(1), -char(4),char(0),char(86),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1),char(8),char(0),char(114),char(1),char(8),char(0),char(115),char(1), -char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(8),char(0),char(118),char(1),char(8),char(0),char(119),char(1),char(8),char(0),char(120),char(1), -char(8),char(0),char(121),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(49),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(92),char(0),char(24),char(0),char(15),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1), -char(13),char(0),char(107),char(1),char(13),char(0),char(108),char(1),char(13),char(0),char(109),char(1),char(4),char(0),char(112),char(1),char(7),char(0),char(110),char(1), -char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(7),char(0),char(114),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1), -char(4),char(0),char(113),char(1),char(7),char(0),char(117),char(1),char(7),char(0),char(118),char(1),char(7),char(0),char(119),char(1),char(7),char(0),char(120),char(1), -char(7),char(0),char(121),char(1),char(7),char(0),char(122),char(1),char(0),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(50),char(0),char(125),char(1), -char(0),char(0),char(126),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(127),char(1),char(14),char(0),char(-128),char(1),char(8),char(0),char(-127),char(1), -char(0),char(0),char(-126),char(1),char(91),char(0),char(90),char(1),char(49),char(0),char(-125),char(1),char(0),char(0),char(126),char(1),char(4),char(0),char(97),char(1), -char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(-126),char(1),char(92),char(0),char(90),char(1),char(50),char(0),char(-125),char(1), -char(19),char(0),char(127),char(1),char(13),char(0),char(-128),char(1),char(7),char(0),char(-127),char(1),char(4),char(0),char(97),char(1),}; +char(13),char(0),char(-53),char(0),char(13),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(71),char(0),char(9),char(0), +char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0), +char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(72),char(0),char(5),char(0), +char(70),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(7),char(0),char(-46),char(0),char(7),char(0),char(-45),char(0), +char(73),char(0),char(5),char(0),char(71),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(8),char(0),char(-46),char(0), +char(8),char(0),char(-45),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0), +char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0), +char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0), +char(13),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0),char(13),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0), +char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0),char(0),char(0),char(-59),char(0),char(13),char(0),char(-53),char(0), +char(13),char(0),char(-52),char(0),char(13),char(0),char(-28),char(0),char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0), +char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0),char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0), +char(13),char(0),char(-19),char(0),char(13),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0), +char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-74),char(0), +char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),char(14),char(0),char(-44),char(0), +char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0), +char(14),char(0),char(-38),char(0),char(14),char(0),char(-37),char(0),char(14),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0), +char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-30),char(0),char(0),char(0),char(-29),char(0), +char(0),char(0),char(-59),char(0),char(14),char(0),char(-53),char(0),char(14),char(0),char(-52),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0), +char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0),char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0), +char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0),char(14),char(0),char(-19),char(0),char(14),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0), +char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(0),char(0),char(-14),char(0),char(0),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(76),char(0),char(9),char(0),char(59),char(0),char(-74),char(0),char(19),char(0),char(-71),char(0),char(19),char(0),char(-70),char(0),char(7),char(0),char(-55),char(0), +char(7),char(0),char(-54),char(0),char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0), +char(77),char(0),char(9),char(0),char(61),char(0),char(-74),char(0),char(20),char(0),char(-71),char(0),char(20),char(0),char(-70),char(0),char(8),char(0),char(-55),char(0), +char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0),char(8),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0), +char(78),char(0),char(5),char(0),char(58),char(0),char(-74),char(0),char(13),char(0),char(-11),char(0),char(13),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0), +char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-74),char(0),char(14),char(0),char(-11),char(0),char(14),char(0),char(-10),char(0), +char(8),char(0),char(-9),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-8),char(0),char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0), +char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0), +char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0),char(13),char(0),char(0),char(1),char(7),char(0),char(-99),char(0),char(7),char(0),char(1),char(1), +char(4),char(0),char(2),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-5),char(0),char(4),char(0),char(3),char(1), +char(7),char(0),char(4),char(1),char(4),char(0),char(5),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(0),char(1),char(80),char(0),char(-5),char(0), +char(4),char(0),char(6),char(1),char(7),char(0),char(7),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(8),char(1),char(80),char(0),char(-5),char(0), +char(4),char(0),char(9),char(1),char(7),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(7),char(0),char(12),char(1),char(4),char(0),char(53),char(0), +char(85),char(0),char(6),char(0),char(17),char(0),char(13),char(1),char(13),char(0),char(11),char(1),char(13),char(0),char(14),char(1),char(60),char(0),char(15),char(1), +char(4),char(0),char(16),char(1),char(7),char(0),char(12),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(17),char(1),char(7),char(0),char(18),char(1), +char(7),char(0),char(127),char(0),char(7),char(0),char(19),char(1),char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1), +char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1), +char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1), +char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),char(7),char(0),char(35),char(1),char(7),char(0),char(36),char(1),char(4),char(0),char(37),char(1), +char(4),char(0),char(38),char(1),char(4),char(0),char(39),char(1),char(4),char(0),char(40),char(1),char(4),char(0),char(120),char(0),char(87),char(0),char(12),char(0), +char(17),char(0),char(41),char(1),char(17),char(0),char(42),char(1),char(17),char(0),char(43),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(45),char(1), +char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1),char(4),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1), +char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(51),char(1),char(17),char(0),char(52),char(1), +char(17),char(0),char(53),char(1),char(13),char(0),char(44),char(1),char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1), +char(13),char(0),char(57),char(1),char(13),char(0),char(58),char(1),char(4),char(0),char(59),char(1),char(7),char(0),char(60),char(1),char(4),char(0),char(61),char(1), +char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1),char(7),char(0),char(64),char(1),char(7),char(0),char(65),char(1),char(4),char(0),char(66),char(1), +char(4),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1), +char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1),char(4),char(0),char(74),char(1),char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1), +char(89),char(0),char(12),char(0),char(9),char(0),char(77),char(1),char(9),char(0),char(78),char(1),char(13),char(0),char(79),char(1),char(7),char(0),char(80),char(1), +char(7),char(0),char(-125),char(0),char(7),char(0),char(81),char(1),char(4),char(0),char(82),char(1),char(13),char(0),char(83),char(1),char(4),char(0),char(84),char(1), +char(4),char(0),char(85),char(1),char(4),char(0),char(86),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-109),char(0), +char(87),char(0),char(87),char(1),char(80),char(0),char(88),char(1),char(81),char(0),char(89),char(1),char(82),char(0),char(90),char(1),char(83),char(0),char(91),char(1), +char(84),char(0),char(92),char(1),char(85),char(0),char(93),char(1),char(88),char(0),char(94),char(1),char(89),char(0),char(95),char(1),char(4),char(0),char(96),char(1), +char(4),char(0),char(62),char(1),char(4),char(0),char(97),char(1),char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1), +char(4),char(0),char(101),char(1),char(4),char(0),char(102),char(1),char(86),char(0),char(103),char(1),char(91),char(0),char(24),char(0),char(16),char(0),char(104),char(1), +char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),char(14),char(0),char(107),char(1),char(14),char(0),char(108),char(1),char(14),char(0),char(109),char(1), +char(8),char(0),char(110),char(1),char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1), +char(8),char(0),char(114),char(1),char(8),char(0),char(115),char(1),char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(8),char(0),char(118),char(1), +char(8),char(0),char(119),char(1),char(8),char(0),char(120),char(1),char(8),char(0),char(121),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(123),char(1), +char(0),char(0),char(124),char(1),char(49),char(0),char(125),char(1),char(0),char(0),char(126),char(1),char(92),char(0),char(24),char(0),char(15),char(0),char(104),char(1), +char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(13),char(0),char(107),char(1),char(13),char(0),char(108),char(1),char(13),char(0),char(109),char(1), +char(4),char(0),char(112),char(1),char(7),char(0),char(110),char(1),char(4),char(0),char(111),char(1),char(4),char(0),char(86),char(1),char(7),char(0),char(114),char(1), +char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(117),char(1),char(7),char(0),char(118),char(1), +char(7),char(0),char(119),char(1),char(7),char(0),char(120),char(1),char(7),char(0),char(121),char(1),char(7),char(0),char(122),char(1),char(0),char(0),char(123),char(1), +char(0),char(0),char(124),char(1),char(50),char(0),char(125),char(1),char(0),char(0),char(126),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(127),char(1), +char(14),char(0),char(-128),char(1),char(8),char(0),char(-127),char(1),char(0),char(0),char(-126),char(1),char(91),char(0),char(90),char(1),char(49),char(0),char(-125),char(1), +char(0),char(0),char(126),char(1),char(4),char(0),char(97),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(-126),char(1), +char(92),char(0),char(90),char(1),char(50),char(0),char(-125),char(1),char(19),char(0),char(127),char(1),char(13),char(0),char(-128),char(1),char(7),char(0),char(-127),char(1), +char(4),char(0),char(97),char(1),char(95),char(0),char(4),char(0),char(50),char(0),char(-124),char(1),char(9),char(0),char(-123),char(1),char(4),char(0),char(-122),char(1), +char(0),char(0),char(37),char(0),char(96),char(0),char(4),char(0),char(49),char(0),char(-124),char(1),char(9),char(0),char(-123),char(1),char(4),char(0),char(-122),char(1), +char(0),char(0),char(37),char(0),}; int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/src/clew/clew.c b/src/clew/clew.c index a07b0aad7..5afc42a48 100644 --- a/src/clew/clew.c +++ b/src/clew/clew.c @@ -15,7 +15,7 @@ typedef HMODULE CLEW_DYNLIB_HANDLE; - #define CLEW_DYNLIB_OPEN LoadLibrary + #define CLEW_DYNLIB_OPEN LoadLibraryA #define CLEW_DYNLIB_CLOSE FreeLibrary #define CLEW_DYNLIB_IMPORT GetProcAddress #else From c8f1efe7dc32bd92d6b83d4741595c2694897c37 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 23 Nov 2017 09:15:19 -0800 Subject: [PATCH 20/30] fix case in include --- .../Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 0473a4a3e..9b0dca0c0 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp @@ -6,7 +6,7 @@ #include "btBulletDynamicsCommon.h" #include "BulletDynamics/Featherstone/btMultiBody.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultibodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" struct btMultiBodyWorldImporterInternalData { From 874d7640512e713eeb03872742c7a1ec3019e671 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 23 Nov 2017 09:32:31 -0800 Subject: [PATCH 21/30] add missing file in CMakeLists.txt --- examples/ExampleBrowser/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index efe7c084e..fdf958dfb 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -259,6 +259,7 @@ SET(BulletExampleBrowser_SRCS ../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp ../Importers/ImportMeshUtility/b3ImportMeshUtility.h ../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp + ../../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp ../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp ../../Extras/Serialize/BulletFileLoader/bChunk.cpp ../../Extras/Serialize/BulletFileLoader/bFile.cpp From e6e3da11e5a7a9abf0f143da7315e17767fce670 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 23 Nov 2017 17:38:23 -0800 Subject: [PATCH 22/30] Implement true implicit friction cone, instead of friction pyramid, for btMultiBody vs btMultiBody and btMultiBody vs btRigidBody See data/sphere_small_zeroinertia.urdf for an example. --- data/sphere_small_zeroinertia.urdf | 30 ++ .../btMultiBodyConstraintSolver.cpp | 304 +++++++++++++++--- .../btMultiBodyConstraintSolver.h | 6 +- 3 files changed, 289 insertions(+), 51 deletions(-) create mode 100644 data/sphere_small_zeroinertia.urdf diff --git a/data/sphere_small_zeroinertia.urdf b/data/sphere_small_zeroinertia.urdf new file mode 100644 index 000000000..67d442605 --- /dev/null +++ b/data/sphere_small_zeroinertia.urdf @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 1e2d07409..7d8d21ef7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -70,26 +70,45 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //solve featherstone frictional contact - for (int j1=0;j1m_multiBodyFrictionContactConstraints.size();j1++) + for (int j1=0;j1m_multiBodyLateralFrictionContactConstraints.size();j1++) { if (iteration < infoGlobal.m_numIterations) { int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyLateralFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse>btScalar(0)) + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual += residual*residual; + j1++; + int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyLateralFrictionContactConstraints[index2]; + btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); + if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse; + resolveConeFrictionConstraintRows(frictionConstraint,frictionConstraintB); + } + } + else + { + //adjust friction limits here + if (totalImpulse > btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual += residual*residual; + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } } } } @@ -100,7 +119,9 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb { m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); - m_multiBodyFrictionContactConstraints.resize(0); + m_multiBodyLateralFrictionContactConstraints.resize(0); + m_multiBodyTorsionalFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); m_data.m_deltaVelocitiesUnitImpulse.resize(0); m_data.m_deltaVelocities.resize(0); @@ -128,49 +149,51 @@ void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; btSolverBody* bodyA = 0; btSolverBody* bodyB = 0; - int ndofA=0; - int ndofB=0; + int ndofA = 0; + int ndofB = 0; if (c.m_multiBodyA) { - ndofA = c.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } else if(c.m_solverBodyIdA >= 0) + ndofA = c.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i]; + } + else if (c.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (c.m_multiBodyB) { - ndofB = c.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } else if(c.m_solverBodyIdB >= 0) + ndofB = c.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i]; + } + else if (c.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - + if (sum < c.m_lowerLimit) { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } - else if (sum > c.m_upperLimit) + else if (sum > c.m_upperLimit) { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_upperLimit; } else @@ -180,33 +203,214 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt if (c.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdA >= 0) + } + else if (c.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); } if (c.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdB >= 0) + } + else if (c.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } return deltaImpulse; } +btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB) +{ + int ndofA=0; + int ndofB=0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 0; + btScalar deltaImpulseB = 0.f; + btScalar sumB = 0.f; + { + deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + if (cB.m_multiBodyA) + { + ndofA = cB.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i]; + } else if(cB.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA]; + deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (cB.m_multiBodyB) + { + ndofB = cB.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i]; + } else if(cB.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB]; + deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulseB -= deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulseB -= deltaVelBDotn*cB.m_jacDiagABInv; + sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; + } + + btScalar deltaImpulseA = 0.f; + btScalar sumA = 0.f; + const btMultiBodySolverConstraint& cA = cA1; + { + { + deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + if (cA.m_multiBodyA) + { + ndofA = cA.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i]; + } else if(cA.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA]; + deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (cA.m_multiBodyB) + { + ndofB = cA.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i]; + } else if(cA.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB]; + deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulseA -= deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulseA -= deltaVelBDotn*cA.m_jacDiagABInv; + sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA; + } + } + + if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit) + { + btScalar angle = btAtan2(sumA,sumB); + btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle)); + btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle)); + + + if (sumA < -sumAclipped) + { + deltaImpulseA = -sumAclipped - cA.m_appliedImpulse; + cA.m_appliedImpulse = -sumAclipped; + } + else if (sumA > sumAclipped) + { + deltaImpulseA = sumAclipped - cA.m_appliedImpulse; + cA.m_appliedImpulse = sumAclipped; + } + else + { + cA.m_appliedImpulse = sumA; + } + + if (sumB < -sumBclipped) + { + deltaImpulseB = -sumBclipped - cB.m_appliedImpulse; + cB.m_appliedImpulse = -sumBclipped; + } + else if (sumB > sumBclipped) + { + deltaImpulseB = sumBclipped - cB.m_appliedImpulse; + cB.m_appliedImpulse = sumBclipped; + } + else + { + cB.m_appliedImpulse = sumB; + } + //deltaImpulseA = sumAclipped-cA.m_appliedImpulse; + //cA.m_appliedImpulse = sumAclipped; + //deltaImpulseB = sumBclipped-cB.m_appliedImpulse; + //cB.m_appliedImpulse = sumBclipped; + } + else + { + cA.m_appliedImpulse = sumA; + cB.m_appliedImpulse = sumB; + } + + if (cA.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cA.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA); + + } + if (cA.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cA.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA); + } + + if (cB.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cB.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB); + } + if (cB.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cB.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB); + } + + return deltaImpulseA+deltaImpulseB; +} + + void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, @@ -871,7 +1075,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyLateralFrictionContactConstraints.expandNonInitializing(); solverConstraint.m_orgConstraint = 0; solverConstraint.m_orgDofIndex = -1; @@ -908,7 +1112,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyRollingFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing(); solverConstraint.m_orgConstraint = 0; solverConstraint.m_orgDofIndex = -1; @@ -1275,11 +1479,11 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i]; writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); - writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep); } } @@ -1300,12 +1504,12 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; - pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral1 = m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; } //do a callback here? } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 489347d87..99b344e29 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -35,7 +35,8 @@ protected: btMultiBodyConstraintArray m_multiBodyNonContactConstraints; btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyLateralFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; btMultiBodyJacobianData m_data; @@ -45,6 +46,9 @@ protected: btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + //solve 2 friction directions and clamp against the implicit friction cone + btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA, const btMultiBodySolverConstraint& cB); + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); From 4cc29008f7a6ee3f3ec24e716cafd1307f8e674b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 23 Nov 2017 17:39:09 -0800 Subject: [PATCH 23/30] bump up pybullet version to 1.7.1 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index f326c8f85..b7422545d 100644 --- a/setup.py +++ b/setup.py @@ -442,7 +442,7 @@ print("-----") setup( name = 'pybullet', - version='1.7.0', + version='1.7.1', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 9936a1cc92bab93795f3a3ebdd6fde87cb4be0d5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 23 Nov 2017 18:14:29 -0800 Subject: [PATCH 24/30] friction cone test --- examples/pybullet/examples/frictionCone.py | 45 ++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 examples/pybullet/examples/frictionCone.py diff --git a/examples/pybullet/examples/frictionCone.py b/examples/pybullet/examples/frictionCone.py new file mode 100644 index 000000000..88e4b966d --- /dev/null +++ b/examples/pybullet/examples/frictionCone.py @@ -0,0 +1,45 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) +useMaximalCoordinates = False + +p.setGravity(0,0,-10) +plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates) + +p.setRealTimeSimulation(0) + + +velocity = 1 +num = 40 +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster +p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) + +for i in range (num): + print("progress:",i,num) + + x = velocity*math.sin(2.*3.1415*float(i)/num) + y = velocity*math.cos(2.*3.1415*float(i)/num) + print("velocity=",x,y) + sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates) + p.changeDynamics(sphere,-1,lateralFriction=0.02) + #p.changeDynamics(sphere,-1,rollingFriction=10) + p.changeDynamics(sphere,-1,linearDamping=0) + p.changeDynamics(sphere,-1,angularDamping=0) + p.resetBaseVelocity(sphere,linearVelocity=[x,y,0]) + + prevPos = [0,0,0] + for i in range (2048): + p.stepSimulation() + pos = p.getBasePositionAndOrientation(sphere)[0] + if (i&64): + p.addUserDebugLine(prevPos,pos,[1,0,0],1) + prevPos = pos + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) + + +while (1): + time.sleep(0.01) \ No newline at end of file From 82b51ac78d8736cb1b8f19ab5bfd5d244def79cd Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Nov 2017 14:33:24 -0800 Subject: [PATCH 25/30] pybullet: fix Windows numpy build --- examples/pybullet/pybullet.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ade0ec1c8..42a43a818 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5997,21 +5997,21 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyObject* pySeg; int i, j, p; + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values + b3GetCameraImageData(sm, &imageData); // TODO(hellojas): error handling if image size is 0 - int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, bytesPerPixel}; npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; - pyResultList = PyTuple_New(5); + pyResultList = PyTuple_New(5); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); @@ -6427,6 +6427,7 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) PyObject* pySeg; int i, j, p; + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values b3GetCameraImageData(sm, &imageData); // TODO(hellojas): error handling if image size is 0 @@ -6434,8 +6435,6 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, bytesPerPixel}; npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; From db10adeae150c1a69e0223335eabb5369c46242c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Nov 2017 14:34:11 -0800 Subject: [PATCH 26/30] bump up pybullet version to 1.7.2 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index b7422545d..1fe7ddb44 100644 --- a/setup.py +++ b/setup.py @@ -442,7 +442,7 @@ print("-----") setup( name = 'pybullet', - version='1.7.1', + version='1.7.2', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From b1c2bdc72a2df8ad640664f81183899a20f922a0 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 24 Nov 2017 16:50:36 -0800 Subject: [PATCH 27/30] pybullet: fix numpy compile issue on Windows, another fix --- examples/pybullet/pybullet.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 42a43a818..71526de92 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5987,7 +5987,7 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { - PyObject* item2; + PyObject* pyResultList; // store 4 elements in this result: width, // height, rgbData, depth @@ -5996,13 +5996,12 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyObject* pyDep; PyObject* pySeg; - int i, j, p; int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values b3GetCameraImageData(sm, &imageData); // TODO(hellojas): error handling if image size is 0 - + { npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, bytesPerPixel}; npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; @@ -6027,7 +6026,9 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_SetItem(pyResultList, 2, pyRGB); PyTuple_SetItem(pyResultList, 3, pyDep); PyTuple_SetItem(pyResultList, 4, pySeg); + } #else //PYBULLET_USE_NUMPY + PyObject* item2; PyObject* pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; @@ -6417,7 +6418,7 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { - PyObject* item2; + PyObject* pyResultList; // store 4 elements in this result: width, // height, rgbData, depth @@ -6426,7 +6427,7 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) PyObject* pyDep; PyObject* pySeg; - int i, j, p; + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values b3GetCameraImageData(sm, &imageData); @@ -6434,7 +6435,7 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) pyResultList = PyTuple_New(5); PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - + { npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, bytesPerPixel}; npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; @@ -6454,7 +6455,9 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) PyTuple_SetItem(pyResultList, 2, pyRGB); PyTuple_SetItem(pyResultList, 3, pyDep); PyTuple_SetItem(pyResultList, 4, pySeg); + } #else //PYBULLET_USE_NUMPY + PyObject* item2; PyObject* pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; From 1c3092afb8ca2fd976ddefaff9549f2a0b84702a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Nov 2017 16:52:03 -0800 Subject: [PATCH 28/30] bump up pybullet to version 1.7.3 (Windows Numpy fix) --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 1fe7ddb44..f8faa2d37 100644 --- a/setup.py +++ b/setup.py @@ -442,7 +442,7 @@ print("-----") setup( name = 'pybullet', - version='1.7.2', + version='1.7.3', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From c5f79fe979b4348a03f90b62ce2aa8e62f476bb2 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 24 Nov 2017 18:09:20 -0800 Subject: [PATCH 29/30] pybullet testrender.py/testrender_np.py examples, improve matplotlib rendering performance (headless DIRECT and OpenGL/GUI) --- examples/pybullet/examples/testrender.py | 68 ++++++++++++--------- examples/pybullet/examples/testrender_np.py | 38 ++++++++---- 2 files changed, 65 insertions(+), 41 deletions(-) diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py index 55eecd07c..a7aad9b47 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -1,48 +1,56 @@ -import numpy as np -import matplotlib.pyplot as plt -import pybullet -pybullet.connect(pybullet.GUI) -pybullet.loadURDF("plane.urdf") +#testrender.py is a bit slower than testrender_np.py: pixels are copied from C to Python one by one + + +import matplotlib.pyplot as plt +import pybullet +import time + +pybullet.connect(pybullet.DIRECT) +pybullet.loadURDF("plane.urdf",[0,0,-1]) pybullet.loadURDF("r2d2.urdf") -camTargetPos = [0.,0.,0.] +camTargetPos = [0,0,0] cameraUp = [0,0,1] cameraPos = [1,1,1] -yaw = 40 -pitch = 10.0 + +pitch = -10.0 roll=0 upAxisIndex = 2 camDistance = 4 pixelWidth = 320 -pixelHeight = 240 +pixelHeight = 200 nearPlane = 0.01 -farPlane = 1000 -lightDirection = [0.4,0.4,0] -lightColor = [.3,.3,.3]#1,1,1]#optional argument +farPlane = 100 + fov = 60 -#img_arr = pybullet.renderImage(pixelWidth, pixelHeight) -#renderImage(w, h, view[16], projection[16]) -#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane) -for pitch in range (0,360,10) : +main_start = time.time() +for yaw in range (0,360,10) : + start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); - #getCameraImage can also use renderer=pybullet.ER_BULLET_HARDWARE_OPENGL - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightAmbientCoeff = 0.3, lightDiffuseCoeff = 0.4, shadow=1, renderer=pybullet.ER_TINY_RENDERER) - w=img_arr[0] - h=img_arr[1] - rgb=img_arr[2] - dep=img_arr[3] - #print 'width = %d height = %d' % (w,h) - # reshape creates np array - np_img_arr = np.reshape(rgb, (h, w, 4)) - np_img_arr = np_img_arr*(1./255.) - #show - plt.imshow(np_img_arr,interpolation='none') - - plt.pause(0.01) + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + stop = time.time() + print ("renderImage %f" % (stop - start)) + + w=img_arr[0] #width of the image, in pixels + h=img_arr[1] #height of the image, in pixels + rgb=img_arr[2] #color data RGB + dep=img_arr[3] #depth data + + print ('width = %d height = %d' % (w,h)) + + #note that sending the data to matplotlib is really slow + + plt.imshow(rgb,interpolation='none') + plt.show(block=False) + plt.pause(0.001) + +main_stop = time.time() + +print ("Total time %f" % (main_stop - main_start)) pybullet.resetSimulation() diff --git a/examples/pybullet/examples/testrender_np.py b/examples/pybullet/examples/testrender_np.py index 9f84fbed9..88da27fc1 100644 --- a/examples/pybullet/examples/testrender_np.py +++ b/examples/pybullet/examples/testrender_np.py @@ -1,37 +1,48 @@ -#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled, otherwise use testrender.py (slower but compatible without numpy) +#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled +#otherwise use testrender.py (slower but compatible without numpy) +#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU) import numpy as np import matplotlib.pyplot as plt import pybullet import time +from pylab import * -pybullet.connect(pybullet.GUI) +ion() +img = standard_normal((50,100)) +image = imshow(img,interpolation='none',animated=True,label="blah") + + +#pybullet.connect(pybullet.GUI) +pybullet.connect(pybullet.DIRECT) +pybullet.loadURDF("plane.urdf",[0,0,-1]) pybullet.loadURDF("r2d2.urdf") camTargetPos = [0,0,0] cameraUp = [0,0,1] cameraPos = [1,1,1] -yaw = 40 -pitch = 10.0 + +pitch = -10.0 roll=0 upAxisIndex = 2 camDistance = 4 -pixelWidth = 1024 -pixelHeight = 768 +pixelWidth = 320 +pixelHeight = 200 nearPlane = 0.01 -farPlane = 1000 +farPlane = 100 fov = 60 main_start = time.time() -for pitch in range (0,360,10) : +while (1): + for yaw in range (0,360,10): start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, [0,1,0],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) stop = time.time() print ("renderImage %f" % (stop - start)) @@ -44,8 +55,13 @@ for pitch in range (0,360,10) : #note that sending the data to matplotlib is really slow - plt.imshow(rgb,interpolation='none') - plt.pause(0.001) + np_img_arr = np.reshape(rgb, (h, w, 4)) + np_img_arr = np_img_arr*(1./255.) + #show + #plt.imshow(np_img_arr,interpolation='none',extent=(0,1600,0,1200)) + image.set_data(np_img_arr) + #draw() + plt.pause(0.0001) main_stop = time.time() From 6baf82d6d83923af38529cdd9d73adf2f5b50f36 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Nov 2017 18:57:16 -0800 Subject: [PATCH 30/30] improve testrender/np to render faster/interactive on Mac with matplotlib --- examples/pybullet/examples/testrender.py | 28 ++++++++++++++----- examples/pybullet/examples/testrender_np.py | 31 +++++++++++++++------ 2 files changed, 43 insertions(+), 16 deletions(-) diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py index a7aad9b47..758882f11 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -6,10 +6,19 @@ import matplotlib.pyplot as plt import pybullet import time +plt.ion() + +img = [[1,2,3]*50]*100#np.random.rand(200, 320) +#img = [tandard_normal((50,100)) +image = plt.imshow(img,interpolation='none',animated=True,label="blah") +ax = plt.gca() + + pybullet.connect(pybullet.DIRECT) pybullet.loadURDF("plane.urdf",[0,0,-1]) pybullet.loadURDF("r2d2.urdf") +pybullet.setGravity(0,0,-10) camTargetPos = [0,0,0] cameraUp = [0,0,1] cameraPos = [1,1,1] @@ -27,7 +36,9 @@ farPlane = 100 fov = 60 main_start = time.time() -for yaw in range (0,360,10) : +while(1): + for yaw in range (0,360,10) : + pybullet.stepSimulation() start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; @@ -40,15 +51,18 @@ for yaw in range (0,360,10) : h=img_arr[1] #height of the image, in pixels rgb=img_arr[2] #color data RGB dep=img_arr[3] #depth data - + #print(rgb) print ('width = %d height = %d' % (w,h)) - #note that sending the data to matplotlib is really slow - - plt.imshow(rgb,interpolation='none') - plt.show(block=False) - plt.pause(0.001) + #note that sending the data using imshow to matplotlib is really slow, so we use set_data + #plt.imshow(rgb,interpolation='none') + image.set_data(rgb) + ax.plot([0]) + #plt.draw() + #plt.show() + plt.pause(0.01) + main_stop = time.time() print ("Total time %f" % (main_stop - main_start)) diff --git a/examples/pybullet/examples/testrender_np.py b/examples/pybullet/examples/testrender_np.py index 88da27fc1..fc8acd310 100644 --- a/examples/pybullet/examples/testrender_np.py +++ b/examples/pybullet/examples/testrender_np.py @@ -7,12 +7,14 @@ import numpy as np import matplotlib.pyplot as plt import pybullet import time -from pylab import * -ion() -img = standard_normal((50,100)) -image = imshow(img,interpolation='none',animated=True,label="blah") +plt.ion() + +img = np.random.rand(200, 320) +#img = [tandard_normal((50,100)) +image = plt.imshow(img,interpolation='none',animated=True,label="blah") +ax = plt.gca() #pybullet.connect(pybullet.GUI) pybullet.connect(pybullet.DIRECT) @@ -22,6 +24,7 @@ pybullet.loadURDF("r2d2.urdf") camTargetPos = [0,0,0] cameraUp = [0,0,1] cameraPos = [1,1,1] +pybullet.setGravity(0,0,-10) pitch = -10.0 @@ -38,7 +41,9 @@ fov = 60 main_start = time.time() while (1): for yaw in range (0,360,10): + pybullet.stepSimulation() start = time.time() + viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); @@ -55,13 +60,21 @@ while (1): #note that sending the data to matplotlib is really slow - np_img_arr = np.reshape(rgb, (h, w, 4)) - np_img_arr = np_img_arr*(1./255.) + #reshape is not needed + #np_img_arr = np.reshape(rgb, (h, w, 4)) + #np_img_arr = np_img_arr*(1./255.) + #show #plt.imshow(np_img_arr,interpolation='none',extent=(0,1600,0,1200)) - image.set_data(np_img_arr) - #draw() - plt.pause(0.0001) + #image = plt.imshow(np_img_arr,interpolation='none',animated=True,label="blah") + + image.set_data(rgb)#np_img_arr) + ax.plot([0]) + #plt.draw() + #plt.show() + plt.pause(0.01) + #image.draw() + main_stop = time.time()