btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
void GameObj::spawn_new_obj(string type, btVector3 pos, btVector3 trav_dir, GameObj* s_obj) {
if(type == "las_shoot"){
//TODO check if z-axis is the trav_dir
btVector3 z_vec(0,0,1);
btVector3 xaxis = z_vec.cross(trav_dir);
xaxis.normalize();
btVector3 zaxis = trav_dir.cross(-xaxis);
zaxis.normalize();
btMatrix3x3 mat = btMatrix3x3( xaxis.x(), trav_dir.x(), zaxis.x(),
xaxis.y(), trav_dir.y(), zaxis.y(),
xaxis.z(), trav_dir.z(), zaxis.z());
btTransform temp_trans(mat);
new_objs.push_back(new GameObj("../res/laser_shot1.obj", "fullb", "GameObj", Y_AXIS, pos, temp_trans.getRotation() ));
new_objs.back()->set_spawn_obj(s_obj);
btRigidBody* body = new_objs.back()->get_body();
body->setGravity(btVector3(0,0,0));
body->setLinearVelocity(trav_dir*75);
}
}
bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
{
btVector3 tri_normal = tri_normal_org;
//we only have a local triangle normal, not a local contact normal -> only normal in world space...
//either compute the current angle all in local space, or all in world space
btVector3 edgeCross = edge.cross(tri_normal).normalize();
btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
if (correctedEdgeAngle<0)
{
if (curAngle < correctedEdgeAngle)
{
btScalar diffAngle = correctedEdgeAngle-curAngle;
btQuaternion rotation(edge,diffAngle );
clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
return true;
}
}
if (correctedEdgeAngle>=0)
{
if (curAngle > correctedEdgeAngle)
{
btScalar diffAngle = correctedEdgeAngle-curAngle;
btQuaternion rotation(edge,diffAngle );
clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
return true;
}
}
return false;
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
/**
* Barycentric method for determining the intersection of a ray and a triangle
* @param ray ray directionа
* @param dot point of ray-plane intersection
*/
bool Portal::rayIntersect(const btVector3& ray, const btVector3& rayStart)
{
if(std::abs(normal.normal.dot(ray)) < SPLIT_EPSILON)
{
// the plane is nearly parallel to the ray
return false;
}
if(-normal.distance(rayStart) <= 0)
{
// plane is on the wrong side of the ray
return false;
}
// The vector that does not change for the entire polygon
const btVector3 T = rayStart - vertices[0];
btVector3 edge = vertices[1] - vertices[0];
// Bypass polygon fan, one of the vectors remains unchanged
for(size_t i = 2; i < vertices.size(); i++)
{
// PREV
btVector3 prevEdge = edge;
edge = vertices[i] - vertices[0];
btVector3 P = ray.cross(edge);
btVector3 Q = T.cross(prevEdge);
btScalar t = P.dot(prevEdge);
btScalar u = P.dot(T) / t;
btScalar v = Q.dot(ray) / t;
t = 1.0f - u - v;
if((u <= 1.0) && (u >= 0.0) && (v <= 1.0) && (v >= 0.0) && (t <= 1.0) && (t >= 0.0))
{
return true;
}
}
return false;
}
static void MotionCallback(int x, int y)
{
int dx = mx - x;
int dy = my - y;
Dir = Dir.normalize();
N = Dir.cross(btVector3(0,1,0));
NxQuat qx(NxPiF32 * dx * 20/ 180.0f, btVector3(0,1,0));
qx.rotate(Dir);
NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N);
qy.rotate(Dir);
mx = x;
my = y;
}
btVector3
tgRodInfo::getConnectionPoint(const btVector3& referencePoint,
const btVector3& destinationPoint,
const double rotation) const
{
if (referencePoint == destinationPoint)
{
throw
std::invalid_argument("Destination point is the reference point.");
}
// Find the closest point on the radius from the referencePoint
const btVector3 cylinderAxis = (getTo() - getFrom()).normalize();
const btVector3 cylinderAxis2 = (getTo() - getFrom()).normalize();
// Vector from reference point to destination point
const btVector3 refToDest =
(referencePoint - destinationPoint).normalize();
// Find a vector perpendicular to both the cylinder axis and refToDest
btVector3 rotationAxis = cylinderAxis.cross(refToDest);
// Handle a vector crossed with itself
if (rotationAxis.length() == 0.0)
{
btScalar a = cylinderAxis[0];
btScalar b = cylinderAxis[1];
btScalar c = cylinderAxis[2];
// Find an arbitrary perpendicular vector
rotationAxis = btVector3(b - c, -a, a).normalize();
}
const btVector3 directional =
cylinderAxis.rotate(rotationAxis, -M_PI / 2.0).normalize();
// Apply one additional rotation so we can end up anywhere we
// want on the radius of the rod
// When added to any point along the cylinder axis, this will take you
// to the surface in the direction of the destinationPoint
const btVector3 surfaceVector = directional.rotate(cylinderAxis2, rotation).normalize()
* m_config.radius;
// Return the the surface point closest to the reference point in the
// direction of the destination point.
return referencePoint + surfaceVector;
}
btMatrix3x3 GetOrientedBasis(btVector3 const &z)
{
btAssert(fabsf(z.length()-1) < 0.01f);
btVector3 t(0,0,0);
if(fabsf(z.z() < 0.999f))
{
t.setZ(1);
}
else
{
t.setX(1);
}
btVector3 x = t.cross(z).normalize();
btVector3 y = z.cross(x).normalize();
return btMatrix3x3( x.x(), y.x(), z.x(),
x.y(), y.y(), z.y(),
x.z(), y.z(), z.z());
}
void btMultiBody::setupRevolute(int i,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &zero_rot_parent_to_this,
const btVector3 &joint_axis,
const btVector3 &parent_axis_position,
const btVector3 &my_axis_position,
bool disableParentCollision)
{
links[i].mass = mass;
links[i].inertia = inertia;
links[i].parent = parent;
links[i].zero_rot_parent_to_this = zero_rot_parent_to_this;
links[i].axis_top = joint_axis;
links[i].axis_bottom = joint_axis.cross(my_axis_position);
links[i].d_vector = my_axis_position;
links[i].e_vector = parent_axis_position;
links[i].is_revolute = true;
if (disableParentCollision)
links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
links[i].updateCache();
}
void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVertexArray worldVertsB2;
btVertexArray* pVtxIn = &worldVertsB1;
btVertexArray* pVtxOut = &worldVertsB2;
pVtxOut->reserve(pVtxIn->size());
int closestFaceA=-1;
{
btScalar dmin = FLT_MAX;
for(int face=0;face<hullA.m_faces.size();face++)
{
const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
btScalar d = faceANormalWS.dot(separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA<0)
return;
const btFace& polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn->size();
int numVerticesA = polyA.m_indices.size();
for(int e0=0;e0<numVerticesA;e0++)
{
const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
const btVector3 edge0 = a - b;
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
btVector3 worldA1 = transA*a;
btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
//int otherFace=0;
#ifdef BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
btVector3 planeNormalWS = planeNormalWS1;
btScalar planeEqWS=planeEqWS1;
#endif
//clip face
clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
btSwap(pVtxIn,pVtxOut);
pVtxOut->resize(0);
}
//#define ONLY_REPORT_DEEPEST_POINT
btVector3 point;
// only keep points that are behind the witness face
{
btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btScalar localPlaneEq = polyA.m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
for (int i=0;i<pVtxIn->size();i++)
{
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
if (depth <=minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <=maxDist)
{
btVector3 point = pVtxIn->at(i);
#ifdef ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if 0
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.addContactPoint(separatingNormal,point,depth);
#endif
}
}
}
#ifdef ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.addContactPoint(separatingNormal,point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
continue;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar dist;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
}
}
}
}
const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
if((deltaC.dot(sep))>0.0f)
sep = -sep;
return true;
}
void btMultiBody::fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
// temporary space
int num_links = getNumLinks();
scratch_v.resize(2*num_links + 2);
scratch_m.resize(num_links + 1);
btVector3 * v_ptr = &scratch_v[0];
btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(num_links);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
const btVector3 p_minus_com_world = contact_point - base_pos;
rot_from_world[0] = btMatrix3x3(base_quat);
p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
n_local[0] = rot_from_world[0] * normal;
// omega coeffients first.
btVector3 omega_coeffs;
omega_coeffs = p_minus_com_world.cross(normal);
jac[0] = omega_coeffs[0];
jac[1] = omega_coeffs[1];
jac[2] = omega_coeffs[2];
// then v coefficients
jac[3] = normal[0];
jac[4] = normal[1];
jac[5] = normal[2];
// Set remaining jac values to zero for now.
for (int i = 6; i < 6 + num_links; ++i) {
jac[i] = 0;
}
// Qdot coefficients, if necessary.
if (num_links > 0 && link > -1) {
// TODO: speed this up -- don't calculate for links we don't need.
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
// which is resulting in repeated work being done...)
// calculate required normals & positions in the local frames.
for (int i = 0; i < num_links; ++i) {
// transform to local frame
const int parent = links[i].parent;
const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
n_local[i+1] = mtx * n_local[parent+1];
p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;
// calculate the jacobian entry
if (links[i].is_revolute) {
results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
} else {
results[i] = n_local[i+1].dot( links[i].axis_bottom );
}
}
// Now copy through to output.
while (link != -1) {
jac[6 + link] = results[link];
link = links[link].parent;
}
}
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
faceANormalWS*=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
WorldNormal *=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d,wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA, witnessPointB;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar dist;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, Cross, dist, wA, wB))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
edgeA=e0;
edgeB=e1;
worldEdgeA = WorldEdge0;
worldEdgeB = WorldEdge1;
witnessPointA=wA;
witnessPointB=wB;
}
}
}
}
if (edgeA>=0&&edgeB>=0)
{
// printf("edge-edge\n");
//add an edge-edge contact
btVector3 ptsVector;
btVector3 offsetA;
btVector3 offsetB;
btScalar tA;
btScalar tB;
btVector3 translation = witnessPointB-witnessPointA;
btVector3 dirA = worldEdgeA;
btVector3 dirB = worldEdgeB;
btScalar hlenB = 1e30f;
btScalar hlenA = 1e30f;
btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
translation,
dirA, hlenA,
dirB, hlenB);
btScalar nlSqrt = ptsVector.length2();
if (nlSqrt>SIMD_EPSILON)
{
btScalar nl = btSqrt(nlSqrt);
ptsVector *= 1.f/nl;
if (ptsVector.dot(DeltaC2)<0.f)
{
ptsVector*=-1.f;
}
btVector3 ptOnB = witnessPointB + offsetB;
btScalar distance = nl;
resultOut.addContactPoint(ptsVector, ptOnB,-distance);
}
}
if((DeltaC2.dot(sep))<0.0f)
sep = -sep;
return true;
}
inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
const btMatrix3x3 &I)
{
const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
return w2;
}
bool EpaFace::CalculatePlane()
{
assert( ( m_pVertices[ 0 ] && m_pVertices[ 1 ] && m_pVertices[ 2 ] )
&& "Must setup vertices pointers first!" );
// Traditional method
const btVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
m_planeNormal = v2.cross( v1 );
if ( m_planeNormal.length2() == 0 )
{
return false;
}
m_planeNormal.normalize();
m_planeDistance = m_pVertices[ 0 ]->m_point.dot( -m_planeNormal );
// Robust method
//btVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
//btVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
//btVector3 n;
//n = _v2.cross( _v1 );
//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 1 ]->m_point;
//_v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 1 ]->m_point;
//n += ( _v1.cross( _v2 ) );
//_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 2 ]->m_point;
//_v2 = m_pVertices[ 1 ]->m_point - m_pVertices[ 2 ]->m_point;
//n += ( _v2.cross( _v1 ) );
//n /= 3;
//n.normalize();
//btVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
//btScalar d = c.dot( -n );
//m_robustPlaneNormal = n;
//m_robustPlaneDistance = d;
// Compare results from both methods and check whether they disagree
//if ( d < 0 )
//{
// assert( ( m_planeDistance < 0 ) && "He he! Busted!" );
//}
//else
//{
// assert( ( m_planeDistance >= 0 ) && "He he! Busted!" );
//}
return true;
}
void updateCamera()
{
btVector3 m_cameraUp(0,1,0);
int m_forwardAxis=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;
float m_frustumZNear=1;
float m_frustumZFar=1000;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
return;
float aspect;
btVector3 extents;
if (m_glutScreenWidth > m_glutScreenHeight)
{
aspect = m_glutScreenWidth / (float)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = m_glutScreenHeight / (float)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;
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
} else
{
if (m_glutScreenWidth > m_glutScreenHeight)
{
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
} else
{
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());
}
}