Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Test conversion from view matrix to yaw pitch roll.

Browse Source
This commit is contained in:
yunfeibai
2017-06-02 16:56:05 -07:00
parent f350a506a6
commit 3506603d60
5 changed files with 29 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/RollingFrictionDemo/RollingFrictionDemo.cpp
View File

@@ -58,9 +58,9 @@ public:
void resetCamera() void resetCamera()
{ {
float dist = 35; float dist = 1;
float pitch = -14; float pitch = 70;
float yaw = 0; float yaw = 10;
float targetPos[3]={0,0,0}; float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]); m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
} }

11
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1960,7 +1960,14 @@ void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const floa
eyeInitPos.normalize(); eyeInitPos.normalize();
eyePos.normalize(); eyePos.normalize();
eyeInitPos[0] = 0.0;
eyeInitPos[1] = -1.0;
eyeInitPos[2] = 0.0;
b3Quaternion rot = b3ShortestArcQuat(eyeInitPos, eyePos); b3Quaternion rot = b3ShortestArcQuat(eyeInitPos, eyePos);
//rot[0] = 0.571393847;
//rot[1] = 0.0499904789;
//rot[2] = 0.0713938028;
//rot[3] = 0.816034972;
btScalar yawRad; btScalar yawRad;
btScalar pitchRad; btScalar pitchRad;
btScalar rollRad; btScalar rollRad;
@@ -2088,7 +2095,9 @@ void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], fl
camForward.normalize(); camForward.normalize();
} }
eyePos = b3Matrix3x3(eyeRot)*eyePos; eyePos[3] = 1.0;
//eyePos = b3Matrix3x3(eyeRot)*eyePos;
eyePos = b3QuatRotate(eyeRot, eyePos);
camUpVector = b3Matrix3x3(eyeRot)*camUpVector; camUpVector = b3Matrix3x3(eyeRot)*camUpVector;
camPos = eyePos; camPos = eyePos;

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -277,7 +277,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
float viewMatrix[16]; 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 distance = 1.0;
float yaw = 10.0; float yaw = 10.0;
float pitch = 70.0; float pitch = 70.0;

8
examples/pybullet/examples/kuka_with_cube_playback.py
View File

@@ -52,10 +52,10 @@ def readLogFile(filename, verbose = True):
#clid = p.connect(p.SHARED_MEMORY) #clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("plane.urdf",[0,0,-0.3])
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) p.loadURDF("kuka_iiwa/model.urdf",[0,0,10])
p.loadURDF("cube.urdf",[2,2,5]) p.loadURDF("cube.urdf",[2,2,50])
p.loadURDF("cube.urdf",[-2,-2,5]) p.loadURDF("cube.urdf",[-2,-2,50])
p.loadURDF("cube.urdf",[2,-2,5]) p.loadURDF("cube.urdf",[2,-2,50])
log = readLogFile("LOG0001.txt") log = readLogFile("LOG0001.txt")

11
src/Bullet3Common/b3Quaternion.h
View File

@@ -887,6 +887,16 @@ b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
B3_FORCE_INLINE b3Quaternion B3_FORCE_INLINE b3Quaternion
b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized 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); b3Vector3 c = v0.cross(v1);
b3Scalar d = v0.dot(v1); b3Scalar d = v0.dot(v1);
@@ -901,6 +911,7 @@ b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming
b3Scalar rs = 1.0f / s; b3Scalar rs = 1.0f / s;
return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
} }
B3_FORCE_INLINE b3Quaternion B3_FORCE_INLINE b3Quaternion