void DemoApplication::updateCamera() {
/*
printf("ele=%f, azi=%f, cp=[%f,%f,%f], ct=[%f,%f,%f], cu=[%f,%f,%f]\n",
m_ele, m_azi,
m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp[0], m_cameraUp[1], m_cameraUp[2]);
*/
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg
btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg
btQuaternion rot(m_cameraUp,razi);
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
return;
btScalar aspect;
btVector3 extents;
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//glTranslatef(100,210,0);
} else
{
// glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
}
void bulletBaseApp::updateCamera() {
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
float rele = m_ele * 0.01745329251994329547;// rads per deg
float razi = m_azi * 0.01745329251994329547;// rads per deg
btQuaternion rot(m_cameraUp,razi);
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
return;
btScalar aspect;
btVector3 extents;
if (m_glutScreenWidth > m_glutScreenHeight)
{
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = m_glutScreenHeight / (btScalar)m_glutScreenWidth;
extents.setValue(1.0f, aspect*1.f,0);
}
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//glTranslatef(100,210,0);
} else
{
if (m_glutScreenWidth > m_glutScreenHeight)
{
glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
} else
{
glFrustum (-1.0, 1.0, -aspect, aspect, 1.0, 10000.0);
}
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
}
void MotionState::getWorldTransform(btTransform& worldTrans) const {
const Matrix4 transformation = object().transformationMatrix();
worldTrans.setOrigin(btVector3(transformation.translation()));
worldTrans.setBasis(btMatrix3x3(transformation.rotationScaling()));
}
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
//skip self-collisions
if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
return;
//skip duplicates (disabled for now)
//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
// return;
//search for shared vertices and edges
int numshared = 0;
int sharedVertsA[3]={-1,-1,-1};
int sharedVertsB[3]={-1,-1,-1};
///skip degenerate triangles
btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
return;
btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
///skip degenerate triangles
if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
return;
#if 0
printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
#endif
for (int i=0;i<3;i++)
{
for (int j=0;j<3;j++)
{
if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
{
sharedVertsA[numshared] = i;
sharedVertsB[numshared] = j;
numshared++;
///degenerate case
if(numshared >= 3)
return;
}
}
///degenerate case
if(numshared >= 3)
return;
}
switch (numshared)
{
case 0:
{
break;
}
case 1:
{
//shared vertex
break;
}
case 2:
{
//shared edge
//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
{
sharedVertsA[0] = 2;
sharedVertsA[1] = 0;
int tmp = sharedVertsB[1];
sharedVertsB[1] = sharedVertsB[0];
sharedVertsB[0] = tmp;
}
int hash = btGetHash(m_partIdA,m_triangleIndexA);
btTriangleInfo* info = m_triangleInfoMap->find(hash);
if (!info)
{
btTriangleInfo tmp;
m_triangleInfoMap->insert(hash,tmp);
info = m_triangleInfoMap->find(hash);
}
int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
int otherIndexA = 3-sumvertsA;
btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
btVector3 normalA;
btVector3 normalB;
tA.calcNormal(normalA);
tB.calcNormal(normalB);
edge.normalize();
btVector3 edgeCrossA = edge.cross(normalA).normalize();
{
btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
if (edgeCrossA.dot(tmp) < 0)
{
edgeCrossA*=-1;
}
}
btVector3 edgeCrossB = edge.cross(normalB).normalize();
{
btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
if (edgeCrossB.dot(tmp) < 0)
{
edgeCrossB*=-1;
}
}
btScalar angle2 = 0;
btScalar ang4 = 0.f;
btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
btScalar len2 = calculatedEdge.length2();
btScalar correctedAngle(0);
btVector3 calculatedNormalB = normalA;
bool isConvex = false;
if (len2<m_triangleInfoMap->m_planarEpsilon)
{
angle2 = 0.f;
ang4 = 0.f;
} else
{
calculatedEdge.normalize();
btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
calculatedNormalA.normalize();
angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
ang4 = SIMD_PI-angle2;
btScalar dotA = normalA.dot(edgeCrossB);
///@todo: check if we need some epsilon, due to floating point imprecision
isConvex = (dotA<0.);
correctedAngle = isConvex ? ang4 : -ang4;
btQuaternion orn2 = btQuaternion(btVector3(calculatedEdge.x(), calculatedEdge.y(), calculatedEdge.z()),-correctedAngle);
calculatedNormalB = btMatrix3x3(orn2)*normalA;
}
//alternatively use
//btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
switch (sumvertsA)
{
case 1:
{
btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
btQuaternion orn(edge,-correctedAngle);
btVector3 computedNormalB = quatRotate(orn,normalA);
btScalar bla = computedNormalB.dot(normalB);
if (bla<0)
{
computedNormalB*=-1;
info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
}
#ifdef DEBUG_INTERNAL_EDGE
if ((computedNormalB-normalB).length()>0.0001)
{
printf("warning: normals not identical\n");
}
#endif//DEBUG_INTERNAL_EDGE
info->m_edgeV0V1Angle = -correctedAngle;
if (isConvex)
info->m_flags |= TRI_INFO_V0V1_CONVEX;
break;
}
case 2:
{
btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
btQuaternion orn(edge,-correctedAngle);
btVector3 computedNormalB = quatRotate(orn,normalA);
if (computedNormalB.dot(normalB)<0)
{
computedNormalB*=-1;
info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
}
#ifdef DEBUG_INTERNAL_EDGE
if ((computedNormalB-normalB).length()>0.0001)
{
printf("warning: normals not identical\n");
}
#endif //DEBUG_INTERNAL_EDGE
info->m_edgeV2V0Angle = -correctedAngle;
if (isConvex)
info->m_flags |= TRI_INFO_V2V0_CONVEX;
break;
}
case 3:
{
btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
btQuaternion orn(edge,-correctedAngle);
btVector3 computedNormalB = quatRotate(orn,normalA);
if (computedNormalB.dot(normalB)<0)
{
info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
computedNormalB*=-1;
}
#ifdef DEBUG_INTERNAL_EDGE
if ((computedNormalB-normalB).length()>0.0001)
{
printf("warning: normals not identical\n");
}
#endif //DEBUG_INTERNAL_EDGE
info->m_edgeV1V2Angle = -correctedAngle;
if (isConvex)
info->m_flags |= TRI_INFO_V1V2_CONVEX;
break;
}
}
break;
}
}
}