Update MultiBody methods.

AndresTraks · Aug 21, 2016 · 5f6be8d · 5f6be8d
1 parent 15c9eed
commit 5f6be8d
Show file tree

Hide file tree

Showing 12 changed files with 89 additions and 51 deletions.
diff --git a/src/AxisSweep3.h b/src/AxisSweep3.h
@@ -20,10 +20,12 @@ namespace BulletSharp
 		AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, unsigned short maxHandles);
 		AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax);
 
-		unsigned short AddHandle(Vector3 aabbMin, Vector3 aabbMax, IntPtr pOwner, short collisionFilterGroup,
-			short collisionFilterMask, Dispatcher^ dispatcher, IntPtr multiSapProxy);
-		unsigned short AddHandle(Vector3 aabbMin, Vector3 aabbMax, IntPtr pOwner, CollisionFilterGroups collisionFilterGroup,
-			CollisionFilterGroups collisionFilterMask, Dispatcher^ dispatcher, IntPtr multiSapProxy);
+		unsigned short AddHandle(Vector3 aabbMin, Vector3 aabbMax, IntPtr pOwner,
+			short collisionFilterGroup, short collisionFilterMask, Dispatcher^ dispatcher,
+			IntPtr multiSapProxy);
+		unsigned short AddHandle(Vector3 aabbMin, Vector3 aabbMax, IntPtr pOwner,
+			CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask,
+			Dispatcher^ dispatcher, IntPtr multiSapProxy);
 		virtual BroadphaseProxy^ CreateProxy(Vector3% aabbMin, Vector3% aabbMax,
 			BroadphaseNativeType shapeType, IntPtr userPtr, short collisionFilterGroup,
 			short collisionFilterMask, Dispatcher^ dispatcher, IntPtr multiSapProxy) override;
@@ -33,7 +35,8 @@ namespace BulletSharp
 		void RemoveHandle(unsigned short handle, Dispatcher^ dispatcher);
 		bool TestAabbOverlap(BroadphaseProxy^ proxy0, BroadphaseProxy^ proxy1);
 		void UnQuantize(BroadphaseProxy^ proxy, Vector3 aabbMin, Vector3 aabbMax);
-		void UpdateHandle(unsigned short handle, Vector3 aabbMin, Vector3 aabbMax, Dispatcher^ dispatcher);
+		void UpdateHandle(unsigned short handle, Vector3 aabbMin, Vector3 aabbMax,
+			Dispatcher^ dispatcher);
 
 		property unsigned short NumHandles
 		{
@@ -73,7 +76,8 @@ namespace BulletSharp
 		void RemoveHandle(unsigned int handle, Dispatcher^ dispatcher);
 		bool TestAabbOverlap(BroadphaseProxy^ proxy0, BroadphaseProxy^ proxy1);
 		void UnQuantize(BroadphaseProxy^ proxy, Vector3 aabbMin, Vector3 aabbMax);
-		void UpdateHandle(unsigned int handle, Vector3 aabbMin, Vector3 aabbMax, Dispatcher^ dispatcher);
+		void UpdateHandle(unsigned int handle, Vector3 aabbMin, Vector3 aabbMax,
+			Dispatcher^ dispatcher);
 
 		property unsigned int NumHandles
 		{

diff --git a/src/BoxCollision.cpp b/src/BoxCollision.cpp
@@ -61,7 +61,7 @@ Aabb::Aabb(Aabb^ other, btScalar margin)
 	_native = new btAABB(*other->_native, margin);
 }
 
-void Aabb::AppyTransform(Matrix trans)
+void Aabb::ApplyTransform(Matrix trans)
 {
 	TRANSFORM_CONV(trans);
 	_native->appy_transform(TRANSFORM_USE(trans));

diff --git a/src/BoxCollision.h b/src/BoxCollision.h
@@ -23,7 +23,7 @@ namespace BulletSharp
 		Aabb(Aabb^ other);
 		Aabb(Aabb^ other, btScalar margin);
 
-		void AppyTransform(Matrix trans);
+		void ApplyTransform(Matrix trans);
 		//void AppyTransformTransCache(BT_BOX_BOX_TRANSFORM_CACHE^ trans);
 		bool CollidePlane(Vector4 plane);
 		bool CollideRay(Vector3 vorigin, Vector3 vdir);

diff --git a/src/BvhTriangleMeshShape.cpp b/src/BvhTriangleMeshShape.cpp
@@ -16,25 +16,26 @@ BvhTriangleMeshShape::BvhTriangleMeshShape(btBvhTriangleMeshShape* native)
 {
 }
 
-BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-	bool buildBvh)
+BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface,
+	bool useQuantizedAabbCompression, bool buildBvh)
 	: TriangleMeshShape(0)
 {
 	UnmanagedPointer = new btBvhTriangleMeshShape(meshInterface->_native, useQuantizedAabbCompression, buildBvh);
 
 	_meshInterface = meshInterface;
 }
 
-BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression)
+BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface,
+	bool useQuantizedAabbCompression)
 	: TriangleMeshShape(0)
 {
 	UnmanagedPointer = new btBvhTriangleMeshShape(meshInterface->_native, useQuantizedAabbCompression);
 
 	_meshInterface = meshInterface;
 }
 
-BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-	Vector3% bvhAabbMin, Vector3% bvhAabbMax, bool buildBvh)
+BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface,
+	bool useQuantizedAabbCompression, Vector3 bvhAabbMin, Vector3 bvhAabbMax, bool buildBvh)
 	: TriangleMeshShape(0)
 {
 	VECTOR3_CONV(bvhAabbMin);
@@ -47,36 +48,8 @@ BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface,
 	_meshInterface = meshInterface;
 }
 
-BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-	Vector3 bvhAabbMin, Vector3 bvhAabbMax, bool buildBvh)
-	: TriangleMeshShape(0)
-{
-	VECTOR3_CONV(bvhAabbMin);
-	VECTOR3_CONV(bvhAabbMax);
-	UnmanagedPointer = new btBvhTriangleMeshShape(meshInterface->_native, useQuantizedAabbCompression,
-		VECTOR3_USE(bvhAabbMin), VECTOR3_USE(bvhAabbMax), buildBvh);
-	VECTOR3_DEL(bvhAabbMin);
-	VECTOR3_DEL(bvhAabbMax);
-
-	_meshInterface = meshInterface;
-}
-
-BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-	Vector3% bvhAabbMin, Vector3% bvhAabbMax)
-	: TriangleMeshShape(0)
-{
-	VECTOR3_CONV(bvhAabbMin);
-	VECTOR3_CONV(bvhAabbMax);
-	UnmanagedPointer = new btBvhTriangleMeshShape(meshInterface->_native, useQuantizedAabbCompression,
-		VECTOR3_USE(bvhAabbMin), VECTOR3_USE(bvhAabbMax));
-	VECTOR3_DEL(bvhAabbMin);
-	VECTOR3_DEL(bvhAabbMax);
-
-	_meshInterface = meshInterface;
-}
-
-BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-	Vector3 bvhAabbMin, Vector3 bvhAabbMax)
+BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface^ meshInterface,
+	bool useQuantizedAabbCompression, Vector3 bvhAabbMin, Vector3 bvhAabbMax)
 	: TriangleMeshShape(0)
 {
 	VECTOR3_CONV(bvhAabbMin);

diff --git a/src/BvhTriangleMeshShape.h b/src/BvhTriangleMeshShape.h
@@ -21,12 +21,8 @@ namespace BulletSharp
 		BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
 			bool buildBvh);
 		BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression);
-		BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-			Vector3% bvhAabbMin, Vector3% bvhAabbMax, bool buildBvh);
 		BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
 			Vector3 bvhAabbMin, Vector3 bvhAabbMax, bool buildBvh);
-		BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
-			Vector3% bvhAabbMin, Vector3% bvhAabbMax);
 		BvhTriangleMeshShape(StridingMeshInterface^ meshInterface, bool useQuantizedAabbCompression,
 			Vector3 bvhAabbMin, Vector3 bvhAabbMax);
 

diff --git a/src/CompoundCompoundCollisionAlgorithm.h b/src/CompoundCompoundCollisionAlgorithm.h
@@ -30,7 +30,6 @@ namespace BulletSharp
 		};
 
 		CompoundCompoundCollisionAlgorithm(CollisionAlgorithmConstructionInfo^ ci,
-			CollisionObjectWrapper^ body0Wrap, CollisionObjectWrapper^ body1Wrap,
-			bool isSwapped);
+			CollisionObjectWrapper^ body0Wrap, CollisionObjectWrapper^ body1Wrap, bool isSwapped);
 	};
 };
diff --git a/src/MultiBody.cpp b/src/MultiBody.cpp
@@ -296,7 +296,7 @@ bool MultiBody::InternalNeedsJointFeedback()
 #pragma managed(push, off)
 void MultiBody_LocalDirToWorld(btMultiBody* body, int i, btVector3* vec, btVector3* result)
 {
-	result = &body->localDirToWorld(i, *vec);
+	*result = body->localDirToWorld(i, *vec);
 }
 #pragma managed(pop)
 Vector3 MultiBody::LocalDirToWorld(int i, Vector3 vec)
@@ -310,10 +310,27 @@ Vector3 MultiBody::LocalDirToWorld(int i, Vector3 vec)
 	return dir;
 }
 
+#pragma managed(push, off)
+void MultiBody_LocalFrameToWorld(btMultiBody* body, int i, btMatrix3x3* mat, btMatrix3x3* result)
+{
+	*result = body->localFrameToWorld(i, *mat);
+}
+#pragma managed(pop)
+Matrix MultiBody::LocalFrameToWorld(int i, Matrix mat)
+{
+	MATRIX3X3_CONV(mat);
+	btMatrix3x3* resultTemp = ALIGNED_NEW(btMatrix3x3);
+	MultiBody_LocalFrameToWorld(_native, i, MATRIX3X3_PTR(mat), resultTemp);
+	Matrix result = Math::BtMatrix3x3ToMatrix(resultTemp);
+	MATRIX3X3_DEL(mat);
+	ALIGNED_FREE(resultTemp);
+	return result;
+}
+
 #pragma managed(push, off)
 void MultiBody_LocalPosToWorld(btMultiBody* body, int i, btVector3* vec, btVector3* result)
 {
-	result = &body->localPosToWorld(i, *vec);
+	*result = body->localPosToWorld(i, *vec);
 }
 #pragma managed(pop)
 Vector3 MultiBody::LocalPosToWorld(int i, Vector3 vec)

diff --git a/src/MultiBody.h b/src/MultiBody.h
@@ -76,6 +76,7 @@ namespace BulletSharp
 		void GoToSleep();
 		bool InternalNeedsJointFeedback();
 		Vector3 LocalDirToWorld(int i, Vector3 vec);
+		Matrix LocalFrameToWorld(int i, Matrix mat);
 		Vector3 LocalPosToWorld(int i, Vector3 vec);
 		void ProcessDeltaVeeMultiDof2();
 #ifndef DISABLE_SERIALIZE

diff --git a/src/MultiBodyJointMotor.cpp b/src/MultiBodyJointMotor.cpp
@@ -37,6 +37,21 @@ void MultiBodyJointMotor::DebugDraw(IDebugDraw^ drawer)
 }
 #endif
 
+void MultiBodyJointMotor::SetPositionTarget(btScalar posTarget, btScalar kp)
+{
+	Native->setPositionTarget(posTarget, kp);
+}
+
+void MultiBodyJointMotor::SetPositionTarget(btScalar posTarget)
+{
+	Native->setPositionTarget(posTarget);
+}
+
+void MultiBodyJointMotor::SetVelocityTarget(btScalar velTarget, btScalar kd)
+{
+	Native->setVelocityTarget(velTarget, kd);
+}
+
 void MultiBodyJointMotor::SetVelocityTarget(btScalar velTarget)
 {
 	Native->setVelocityTarget(velTarget);

diff --git a/src/MultiBodyJointMotor.h b/src/MultiBodyJointMotor.h
@@ -20,6 +20,10 @@ namespace BulletSharp
 #ifndef DISABLE_DEBUGDRAW
 		virtual void DebugDraw(IDebugDraw^ drawer) override;
 #endif
+
+		void SetPositionTarget(btScalar posTarget, btScalar kp);
+		void SetPositionTarget(btScalar posTarget);
+		void SetVelocityTarget(btScalar velTarget, btScalar kd);
 		void SetVelocityTarget(btScalar velTarget);
 	};
 };
diff --git a/src/MultiBodyLink.cpp b/src/MultiBodyLink.cpp
@@ -214,6 +214,15 @@ void MultiBodyLink::InertiaLocal::set(Vector3 value)
 {
 	Math::Vector3ToBtVector3(value, &_native->m_inertiaLocal);
 }
+
+btScalar MultiBodyLink::JointDamping::get()
+{
+	return _native->m_jointDamping;
+}
+void MultiBodyLink::JointDamping::set(btScalar value)
+{
+	_native->m_jointDamping = value;
+}
 /*
 MultiBodyJointFeedback^ MultiBodyLink::JointFeedback::get()
 {
@@ -224,6 +233,15 @@ void MultiBodyLink::JointFeedback::set(MultiBodyJointFeedback^ value)
 	_native->m_jointFeedback = value->_native;
 }
 */
+btScalar MultiBodyLink::JointFriction::get()
+{
+	return _native->m_jointFriction;
+}
+void MultiBodyLink::JointFriction::set(btScalar value)
+{
+	_native->m_jointFriction = value;
+}
+
 String^ MultiBodyLink::JointName::get()
 {
 	return StringConv::UnmanagedToManaged(_native->m_jointName);

diff --git a/src/MultiBodyLink.h b/src/MultiBodyLink.h
@@ -139,13 +139,24 @@ namespace BulletSharp
 			Vector3 get();
 			void set(Vector3 value);
 		}
+
+		property btScalar JointDamping
+		{
+			btScalar get();
+			void set(btScalar value);
+		}
 		/*
 		property MultiBodyJointFeedback^ JointFeedback
 		{
 			MultiBodyJointFeedback^ get();
 			void set(MultiBodyJointFeedback^ value);
 		}
 		*/
+		property btScalar JointFriction
+		{
+			btScalar get();
+			void set(btScalar value);
+		}
 		property String^ JointName
 		{
 			String^ get();