// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
{
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = axis1.normalize();
btVector3 yAxis = axis2.normalize();
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(anchor);
// now get constraint frame in local coordinate systems
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
// sei limits
setLinearLowerLimit(btVector3(0., 0., 0.));
setLinearUpperLimit(btVector3(0., 0., 0.));
setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
}
void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
// the swing axis is computed as the "twist-free" cone rotation,
// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
// so, if we're outside the limits, the closest way back inside the cone isn't
// along the vector back to the center. better (and more stable) to use the ellipse normal.
// convert swing axis to direction from center to surface of ellipse
// (ie. rotate 2D vector by PI/2)
btScalar y = -vSwingAxis.z();
btScalar z = vSwingAxis.y();
// do the math...
if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
{
// compute gradient/normal of ellipse surface at current "point"
btScalar grad = y/z;
grad *= m_swingSpan2 / m_swingSpan1;
// adjust y/z to represent normal at point (instead of vector to point)
if (y > 0)
y = fabs(grad * z);
else
y = -fabs(grad * z);
// convert ellipse direction back to swing axis
vSwingAxis.setZ(-y);
vSwingAxis.setY( z);
vSwingAxis.normalize();
}
}
// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
btScalar& swingAngle, // out
btVector3& vSwingAxis, // out
btScalar& swingLimit) // out
{
swingAngle = qCone.getAngle();
if (swingAngle > SIMD_EPSILON)
{
vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
vSwingAxis.normalize();
#if 0
// non-zero twist?! this should never happen.
btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
// Compute limit for given swing. tricky:
// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
// For starters, compute the direction from center to surface of ellipse.
// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
btScalar xEllipse = vSwingAxis.y();
btScalar yEllipse = -vSwingAxis.z();
// Now, we use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
if (fabs(xEllipse) > SIMD_EPSILON)
{
btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2);
}
// test!
/*swingLimit = m_swingSpan2;
if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
{
btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
btScalar phi = asin(sinphi);
btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
btScalar alpha = 3.14159f - theta - phi;
btScalar sinalpha = sin(alpha);
swingLimit = m_swingSpan1 * sinphi/sinalpha;
}*/
}
bool Raytracer::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{
btScalar closestHitResults = 1.f;
bool hasHit = false;
btConvexCast::CastResult rayResult;
btSphereShape pointShape(0.0f);
btTransform rayFromTrans;
btTransform rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
//do some culling, ray versus aabb
btVector3 aabbMin,aabbMax;
shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
btScalar hitLambda = 1.f;
btVector3 hitNormal;
btCollisionObject tmpObj;
tmpObj.setWorldTransform(transforms[s]);
if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
{
//reset previous result
//choose the continuous collision detection method
btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0);
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
{
if (rayResult.m_fraction < closestHitResults)
{
closestHitResults = rayResult.m_fraction;
worldNormal = transforms[s].getBasis() *rayResult.m_normal;
worldNormal.normalize();
hasHit = true;
}
}
}
}
return hasHit;
}
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
m_anchor(anchor),
m_axis1(axis1),
m_axis2(axis2)
{
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
btVector3 zAxis = axis1.normalize();
btVector3 xAxis = axis2.normalize();
btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
btTransform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
frameInW.setOrigin(anchor);
// now get constraint frame in local coordinate systems
m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
// sei limits
setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
// like front wheels of a car
setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
// enable suspension
enableSpring(2, true);
setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
setDamping(2, 0.01f);
setEquilibriumPoint();
}
bool Raytracer::singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{
btScalar closestHitResults = 1.f;
ClosestRayResultCallback resultCallback(rayFrom,rayTo);
bool hasHit = false;
btConvexCast::CastResult rayResult;
btSphereShape pointShape(0.0f);
btTransform rayFromTrans;
btTransform rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
//comment-out next line to get all hits, instead of just the closest hit
//resultCallback.m_closestHitFraction = 1.f;
//do some culling, ray versus aabb
btVector3 aabbMin,aabbMax;
shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
btScalar hitLambda = 1.f;
btVector3 hitNormal;
btCollisionObject tmpObj;
tmpObj.setWorldTransform(transforms[s]);
if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
{
//reset previous result
btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, &tmpObj, shapePtr[s], transforms[s], resultCallback);
if (resultCallback.hasHit())
{
//float fog = 1.f - 0.1f * rayResult.m_fraction;
resultCallback.m_hitNormalWorld.normalize();//.m_normal.normalize();
worldNormal = resultCallback.m_hitNormalWorld;
//worldNormal = transforms[s].getBasis() *rayResult.m_normal;
worldNormal.normalize();
hasHit = true;
}
}
}
return hasHit;
}
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;
}
// dir = 1 for CCW search or clockwise around obstacle
// dir = -1 for CW search or counter clockwise around obstacle
bool cSpace::movePointAroundCSpace(btVector3& pt, btVector3 startVect, float stpMag, int dir)
{
float angle=0;
float zHeight = pt.z();
pt.setZ(0);
btVector3 nudge = stpMag * startVect.normalize();
btVector3 npt = pt + nudge;
while(isPointInsideCSpace(npt)) // check if the point is inside of the object
{
npt = pt + nudge.rotate(btVector3(0,0,1),angle); // calculate a new point a radius of vector nudge away at angle
angle += (dir * 0.1); // in radians
if(fabs(angle) > TWOPI) // if the search has gone a full rotation
return false; // and there is no path outside a C-Space return false, stuck
}
pt = npt;
pt.setZ(zHeight);
return true;
}
// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
btScalar& twistAngle, // out
btVector3& vTwistAxis) // out
{
btQuaternion qMinTwist = qTwist;
twistAngle = qTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = operator-(qTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle < 0)
{
// this should never happen
int wtf = 0; wtf = wtf;
}
vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
if (twistAngle > SIMD_EPSILON)
vTwistAxis.normalize();
}
///combined discrete/continuous sphere-triangle
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
{
const btVector3* vertices = &m_triangle->getVertexPtr(0);
const btVector3& c = sphereCenter;
btScalar r = m_sphere->getRadius();
btVector3 delta (0,0,0);
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize();
btVector3 p1ToCentre = c - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal);
if (distanceFromPlane < btScalar(0.))
{
//triangle facing the other way
distanceFromPlane *= btScalar(-1.);
normal *= btScalar(-1.);
}
btScalar contactMargin = contactBreakingThreshold;
bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
bool isInsideShellPlane = distanceFromPlane < r;
btScalar deltaDotNormal = delta.dot(normal);
if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
return false;
// Check for contact / intersection
bool hasContact = false;
btVector3 contactPoint;
if (isInsideContactPlane) {
if (facecontains(c,vertices,normal)) {
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = c - normal*distanceFromPlane;
} else {
// Could be inside one of the contact capsules
btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
btVector3 nearestOnEdge;
for (int i = 0; i < m_triangle->getNumEdges(); i++) {
btVector3 pa;
btVector3 pb;
m_triangle->getEdge(i,pa,pb);
btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr) {
// Yep, we're inside a capsule
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
if (hasContact) {
btVector3 contactToCentre = c - contactPoint;
btScalar distanceSqr = contactToCentre.length2();
if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
btScalar distance = btSqrt(distanceSqr);
resultNormal = contactToCentre;
resultNormal.normalize();
point = contactPoint;
depth = -(r-distance);
return true;
}
if (delta.dot(contactToCentre) >= btScalar(0.0))
return false;
// Moving towards the contact point -> collision
point = contactPoint;
timeOfImpact = btScalar(0.0);
return true;
}
return false;
}
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
{
const btVector3* vertices = &m_triangle->getVertexPtr(0);
btScalar radius = m_sphere->getRadius();
btScalar radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize();
btVector3 p1ToCentre = sphereCenter - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal);
if (distanceFromPlane < btScalar(0.))
{
//triangle facing the other way
distanceFromPlane *= btScalar(-1.);
normal *= btScalar(-1.);
}
bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
// Check for contact / intersection
bool hasContact = false;
btVector3 contactPoint;
if (isInsideContactPlane) {
if (facecontains(sphereCenter,vertices,normal)) {
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = sphereCenter - normal*distanceFromPlane;
} else {
// Could be inside one of the contact capsules
btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
btVector3 nearestOnEdge;
for (int i = 0; i < m_triangle->getNumEdges(); i++) {
btVector3 pa;
btVector3 pb;
m_triangle->getEdge(i,pa,pb);
btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr) {
// Yep, we're inside a capsule
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
if (hasContact) {
btVector3 contactToCentre = sphereCenter - contactPoint;
btScalar distanceSqr = contactToCentre.length2();
if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
{
if (distanceSqr>SIMD_EPSILON)
{
btScalar distance = btSqrt(distanceSqr);
resultNormal = contactToCentre;
resultNormal.normalize();
point = contactPoint;
depth = -(radius-distance);
} else
{
btScalar distance = 0.f;
resultNormal = normal;
point = contactPoint;
depth = -radius;
}
return true;
}
}
return false;
}