From 3506603d606d1ceea55f79848dd6cee3878e656a Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Fri, 2 Jun 2017 16:56:05 -0700
Subject: [PATCH] Test conversion from view matrix to yaw pitch roll.

---
 examples/RollingFrictionDemo/RollingFrictionDemo.cpp  |  6 +++---
 examples/SharedMemory/PhysicsClientC_API.cpp          | 11 ++++++++++-
 examples/SharedMemory/PhysicsClientExample.cpp        |  2 +-
 examples/pybullet/examples/kuka_with_cube_playback.py |  8 ++++----
 src/Bullet3Common/b3Quaternion.h                      | 11 +++++++++++
 5 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp
index be837ab5a..5e13a1ca6 100644
--- a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp
+++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp
@@ -58,9 +58,9 @@ public:
 	
 	void resetCamera()
 	{
-		float dist = 35;
-		float pitch = -14;
-		float yaw = 0;
+		float dist = 1;
+		float pitch = 70;
+		float yaw = 10;
 		float targetPos[3]={0,0,0};
 		m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
 	}
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index c9ee26cda..da153af77 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1960,7 +1960,14 @@ void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const floa
 	eyeInitPos.normalize();
 	eyePos.normalize();
 	
+	eyeInitPos[0] = 0.0;
+	eyeInitPos[1] = -1.0;
+	eyeInitPos[2] = 0.0;
 	b3Quaternion rot = b3ShortestArcQuat(eyeInitPos, eyePos);
+	//rot[0] = 0.571393847;
+	//rot[1] = 0.0499904789;
+	//rot[2] = 0.0713938028;
+	//rot[3] = 0.816034972;
 	btScalar yawRad;
 	btScalar pitchRad;
 	btScalar rollRad;
@@ -2088,7 +2095,9 @@ void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], fl
 		camForward.normalize();
 	}
 	
-	eyePos = b3Matrix3x3(eyeRot)*eyePos;
+	eyePos[3] = 1.0;
+	//eyePos = b3Matrix3x3(eyeRot)*eyePos;
+	eyePos = b3QuatRotate(eyeRot, eyePos);
 	camUpVector = b3Matrix3x3(eyeRot)*camUpVector;
 	
 	camPos = eyePos;
diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp
index df914fafe..e8783f265 100644
--- a/examples/SharedMemory/PhysicsClientExample.cpp
+++ b/examples/SharedMemory/PhysicsClientExample.cpp
@@ -277,7 +277,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
             
 						float viewMatrix[16];
 			
-			float cameraTargetPosition[3] = {1.0, 2.0, 3.0};
+			float cameraTargetPosition[3] = {0.0, 0.0, 0.0};
 			float distance = 1.0;
 			float yaw = 10.0;
 			float pitch = 70.0;
diff --git a/examples/pybullet/examples/kuka_with_cube_playback.py b/examples/pybullet/examples/kuka_with_cube_playback.py
index 12fdcebdd..8e80e73cd 100644
--- a/examples/pybullet/examples/kuka_with_cube_playback.py
+++ b/examples/pybullet/examples/kuka_with_cube_playback.py
@@ -52,10 +52,10 @@ def readLogFile(filename, verbose = True):
 #clid = p.connect(p.SHARED_MEMORY)
 p.connect(p.GUI)
 p.loadURDF("plane.urdf",[0,0,-0.3])
-p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
-p.loadURDF("cube.urdf",[2,2,5])
-p.loadURDF("cube.urdf",[-2,-2,5])
-p.loadURDF("cube.urdf",[2,-2,5])
+p.loadURDF("kuka_iiwa/model.urdf",[0,0,10])
+p.loadURDF("cube.urdf",[2,2,50])
+p.loadURDF("cube.urdf",[-2,-2,50])
+p.loadURDF("cube.urdf",[2,-2,50])
 
 log = readLogFile("LOG0001.txt")
 
diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h
index 0b7564dcd..ee8e7ed57 100644
--- a/src/Bullet3Common/b3Quaternion.h
+++ b/src/Bullet3Common/b3Quaternion.h
@@ -887,6 +887,16 @@ b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
 B3_FORCE_INLINE b3Quaternion 
 b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
 {
+	/*
+	b3Quaternion q;
+	b3Vector3 a = v0.cross(v1);
+	q[0] = a[0];
+	q[1] = a[1];
+	q[2] = a[2];
+	q[3] = b3Sqrt((v0.length()*v0.length()) * (v1.length()*v1.length())) + v0.dot(v1);
+	return q;
+	*/
+	
 	b3Vector3 c = v0.cross(v1);
 	b3Scalar  d = v0.dot(v1);
 
@@ -901,6 +911,7 @@ b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming
 	b3Scalar rs = 1.0f / s;
 
 	return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+	
 }
 
 B3_FORCE_INLINE b3Quaternion