// RotateToMap returns a rotation map which rotates fromVec to have the
// same direction as toVec.
// fromVec and toVec should be unit vectors
RotationMapR3 RotateToMap(const VectorR3& fromVec, const VectorR3& toVec)
{
VectorR3 crossVec = fromVec;
crossVec *= toVec;
double sintheta = crossVec.Norm();
double costheta = fromVec ^ toVec;
if (sintheta <= 1.0e-40)
{
if (costheta > 0.0)
{
return (RotationMapR3(
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0));
}
else
{
GetOrtho(toVec, crossVec); // Get arbitrary orthonormal vector
return (VrRotate(costheta, sintheta, crossVec));
}
}
else
{
crossVec /= sintheta; // Normalize the vector
return (VrRotate(costheta, sintheta, crossVec));
}
}
// PreCalcInfo takes the vertex values and computes information to
// help with intersections with rays.
void ViewableTriangle::PreCalcInfo()
{
VectorR3 EdgeAB = VertexB - VertexA;
VectorR3 EdgeBC = VertexC - VertexB;
VectorR3 EdgeCA = VertexA - VertexC;
if ( (EdgeAB^EdgeBC) < (EdgeBC^EdgeCA) ) {
Normal = EdgeAB*EdgeBC;
}
else {
Normal = EdgeBC*EdgeCA;
}
double mag = Normal.Norm();
if ( mag>0.0 ) {
Normal /= mag; // Unit vector to triangle's plane
}
PlaneCoef = (Normal^VertexA); // Same coef for all three vertices.
double A = EdgeAB.NormSq();
double B = (EdgeAB^EdgeCA);
double C = EdgeCA.NormSq();
double Dinv = 1.0/(A*C-B*B);
A *= Dinv;
B *= Dinv;
C *= Dinv;
Ubeta = EdgeAB;
Ubeta *= C;
Ubeta.AddScaled( EdgeCA, -B );
Ugamma = EdgeCA;
Ugamma *= -A;
Ugamma.AddScaled( EdgeAB, B );
}
// Precalculations for intersection testing speed
void ViewableParallelogram::PreCalcInfo()
{
VectorR3 EdgeAB = VertexB - VertexA;
VectorR3 EdgeBC = VertexC - VertexB;
LengthAB = EdgeAB.Norm();
LengthBC = EdgeBC.Norm();
VertexD = VertexA;
VertexD += EdgeBC; // The fourth vertex
EdgeAB /= LengthAB; // Normalize
EdgeBC /= LengthBC; // Normalize
Normal = EdgeAB*EdgeBC;
double mag = Normal.Norm();
if ( mag>0.0 && LengthAB>0.0 && LengthBC>0.0 ) {
Normal /= mag; // Unit Vector to parallelogram
}
else {
Normal.SetZero(); // In this case, the parallelogram is malformed.
}
PlaneCoef = VertexB^Normal;
NormalAB = Normal*EdgeAB; // Normal in from edge AB
NormalBC = Normal*EdgeBC; // Normal in from edge BC
NormalAB.ReNormalize(); // Just in case
NormalBC.ReNormalize();
CoefAB = VertexA^NormalAB; // Coef. for edge AB.
CoefBC = VertexB^NormalBC;
CoefCD = VertexC^NormalAB;
CoefDA = VertexA^NormalBC;
}
void Arrow( const VectorR3& tail, const VectorR3& head )
{
float t[3];
float h[3];
tail.Dump( t );
head.Dump( h );
Arrow( t, h );
}
// Returns a vector v orthonormal to unit vector u
void GetOrtho( const VectorR3& u, VectorR3& v )
{
if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
v.Set ( u.y, -u.x, 0.0 );
}
else {
v.Set ( 0.0, u.z, -u.y);
}
v.Normalize();
return;
}
Quaternion2 Quaternion2::DecomposeRotation2(const VectorR3 vB) const {
//we need to compute v in A's coordinates
VectorR3 vA = this->rotate(vB);
vA.Normalize();
double temp = 0;
//compute the rotation that aligns the vector v in the two coordinate frames (A and T)
VectorR3 rotAxis = vA * vB;
rotAxis.Normalize();
double rotAngle = -safeACOS(vA.Dot(vB));
Quaternion2 TqA = getRotationQuaternion2(rotAngle, rotAxis*(-1));
return TqA * (*this);
}
// Returns a righthanded orthonormal basis to complement vector u
void GetOrtho( const VectorR3& u, VectorR3& v, VectorR3& w)
{
if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
v.Set ( u.y, -u.x, 0.0 );
}
else {
v.Set ( 0.0, u.z, -u.y);
}
v.Normalize();
w = u;
w *= v;
w.Normalize();
// w.NormalizeFast();
return;
}
RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec)
{
VectorR3 crossVec = fromVec;
crossVec *= toVec;
double sinetheta = crossVec.Norm(); // Not yet normalized!
if ( sinetheta < 1.0e-40 ) {
return ( RotationMapR3(
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0) );
}
crossVec /= sinetheta; // Normalize the vector
double scale = 1.0/sqrt(fromVec.NormSq()*toVec.NormSq());
sinetheta *= scale;
double cosinetheta = (fromVec^toVec)*scale;
return ( VrRotate( cosinetheta, sinetheta, crossVec) );
}
void NffFileLoader::ProcessConeCylNFF( const VectorR3& baseCenter, double baseRadius,
const VectorR3& topCenter, double topRadius )
{
bool isCone = (topRadius==0.0);
if ( !isCone && topRadius!=baseRadius ) {
UnsupportedTruncatedCone();
if ( topRadius<0.5*baseRadius ) {
isCone = true; // Render as a true code
}
else {
topRadius = baseRadius; // Render as a cylinder
}
}
VectorR3 centerLine = topCenter;
centerLine -= baseCenter;
double height = centerLine.Norm();
if ( height==0.0 ) {
return; // We do not support zero height cylinders (Nor does NFF!)
}
centerLine /= height; // Normalize
if ( isCone ) {
ViewableCone* vc = new ViewableCone();
vc->SetApex(topCenter);
vc->SetCenterAxis(centerLine);
vc->SetSlope( baseRadius/height );
vc->SetHeight( height );
ScenePtr->AddViewable(vc);
}
else {
// Create a cylinder
ViewableCylinder* vc = new ViewableCylinder();
vc->SetCenterAxis(centerLine);
centerLine = topCenter;
centerLine += baseCenter;
centerLine *= 0.5;
vc->SetCenter( centerLine );
vc->SetRadius( baseRadius );
vc->SetHeight( height );
ScenePtr->AddViewable(vc);
}
}
bool ViewableSphere::CalcPartials( const VisiblePoint& visPoint,
VectorR3& retPartialU, VectorR3& retPartialV ) const
{
retPartialV = visPoint.GetPosition();
retPartialV -= Center;
retPartialU = retPartialV;
retPartialU *= AxisC; // Magnitude = R sin(phi).
retPartialU *= -PI2; // Adjust for [0,1] domain instead of [-pi,pi]
retPartialV *= retPartialU; // Pointing in right direction, magnitude 2 pi R^2 sin(phi)
// Sign and magnitude adjusted below.
double badMagSq = retPartialV.NormSq();
double h;
switch ( uvProjectionType ) {
case 0: // Spherical projection
h = sqrt(badMagSq);
if ( h==0.0 ) {
retPartialV = AxisB;
return false;
}
retPartialV *= (PI*Radius/h); // Convert to domain [0,1] instead of [-pi/2,pi/2].
// Compensate for sign and R^2 magnitude.
break;
case 1:
h = 2.0*(visPoint.GetV()-0.5); // In range [-1,1]
h = sqrt(badMagSq*(1.0 - h*h)); // Derivative of arcsin(h) = 1/sqrt(1-h^2)
if ( h==0 ) {
retPartialV = AxisB;
return false;
}
retPartialV *= PI*Radius/h; // Adjust sign and magnitude
break;
}
return true;
}
// Returns an intersection if found with distance maxDistance
// viewDir must be a unit vector.
// intersectDistance and visPoint are returned values.
bool ViewableEllipsoid::FindIntersectionNT (
const VectorR3& viewPos, const VectorR3& viewDir, double maxDistance,
double *intersectDistance, VisiblePoint& returnedPoint ) const
{
VectorR3 v = viewPos;
v -= Center;
double pdotuA = v^AxisA;
double pdotuB = v^AxisB;
double pdotuC = v^AxisC;
double udotuA = viewDir^AxisA;
double udotuB = viewDir^AxisB;
double udotuC = viewDir^AxisC;
double C = Square(pdotuA) + Square(pdotuB) + Square(pdotuC) - 1.0;
double B = ( pdotuA*udotuA + pdotuB*udotuB + pdotuC*udotuC );
if ( C>0.0 && B>=0.0 ) {
return false; // Pointing away from the ellipsoid
}
B += B; // Double B to get final factor of 2.
double A = Square(udotuA) + Square(udotuB) + Square(udotuC);
double alpha1, alpha2;
int numRoots = QuadraticSolveRealSafe( A, B, C, &alpha1, &alpha2 );
if ( numRoots==0 ) {
return false;
}
if ( alpha1>0.0 ) {
if ( alpha1>=maxDistance ) {
return false; // Too far away
}
// Found an intersection from outside.
returnedPoint.SetFrontFace();
returnedPoint.SetMaterial( *OuterMaterial );
*intersectDistance = alpha1;
}
else if ( numRoots==2 && alpha2>0.0 && alpha2<maxDistance ) {
// Found an intersection from inside.
returnedPoint.SetBackFace();
returnedPoint.SetMaterial( *InnerMaterial );
*intersectDistance = alpha2;
}
else {
return false; // Both intersections behind us (should never get here)
}
// Calculate intersection position
v=viewDir;
v *= (*intersectDistance);
v += viewPos;
returnedPoint.SetPosition( v ); // Intersection Position
v -= Center; // Now v is the relative position
double vdotuA = v^AxisA;
double vdotuB = v^AxisB;
double vdotuC = v^AxisC;
v = vdotuA*AxisA + vdotuB*AxisB + vdotuC*AxisC;
v.Normalize();
returnedPoint.SetNormal( v );
// Calculate u-v coordinates
ViewableSphere::CalcUV( vdotuB, vdotuC, vdotuA, uvProjectionType,
&returnedPoint.GetUV() );
returnedPoint.SetFaceNumber( 0 );
return true;
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
ikJacobian.Reset();
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
ikJacobian.SetJtargetActive();
}
else {
ikJacobian.SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
if (useAngularPart)
{
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
if (angle > PI) {
angle -= 2.0*PI;
}
else if (angle < -PI) {
angle += 2.0*PI;
}
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
}
{
if (useAngularPart)
{
VectorRn deltaC(6);
MatrixRmn completeJacobian(6,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
} else
{
VectorRn deltaC(3);
MatrixRmn completeJacobian(3,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
}
}
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//ikJacobian.UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
// Use for velocity IK
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
m_dampingCoeff.SetZero();
}
bool ViewableTorus::FindIntersectionNT (
const VectorR3& viewPos, const VectorR3& viewDir,
double maxDistance, double *intersectDistance,
VisiblePoint& returnedPoint ) const
{
// Precheck for collisions by
// checking if passes to within a bounding box
bool insideFlag = true; // Whether viewPos is inside bounding box
double minDistBack = DBL_MAX; // min. distance to a backplane bounding the box
double maxDistFront= -DBL_MAX; // mix. distance to a frontplane bounding the box
double pdotn;
double alpha;
pdotn = (viewPos^AxisC) - CenterCoefC;
alpha = viewDir^AxisC;
if ( !CollideTwoPlanes(pdotn, alpha, MinorRadius,
&insideFlag, &minDistBack, &maxDistFront) ) {
return false;
}
pdotn = (viewPos^AxisA) - CenterCoefA;
alpha = viewDir^AxisA;
if ( !CollideTwoPlanes(pdotn, alpha, OuterRadius,
&insideFlag, &minDistBack, &maxDistFront) ) {
return false;
}
pdotn = (viewPos^AxisB) - CenterCoefB;
alpha = viewDir^AxisB;
if ( !CollideTwoPlanes(pdotn, alpha, OuterRadius,
&insideFlag, &minDistBack, &maxDistFront) ) {
return false;
}
assert ( minDistBack>=maxDistFront );
assert ( insideFlag || maxDistFront>=0.0 );
assert ( maxDistFront < 1000.0 );
if (maxDistFront>maxDistance) {
return false;
}
// Bounding box precheck is done. Now do the actual intersection test.
// Set up the degree 4 polynomial for the torus intersection
// Coefficients are:
// A = 1
// B = 4 u \cdot p
// C = 4(u\cdot p)^2 + 2(p\cdot p) - 2(M^2+m^2) + 4M^2(u_C \cdot u)
// D = 4[(p \cdot p)(u \cdot p) - (M^2+m^2)(u \cdot p) + 2M^2(u_C \dot p)^2]
// E = ((p \cdot p)^2 - 2(M^2+m^2)(p \cdot p) + 4M^2(u_C\cdot p)^2 +(M^2-m^2)^2
double moveFwdDist = Max(0.0,maxDistFront);
double coefs[5];
double roots[4];
double &A=coefs[0], &B=coefs[1], &C=coefs[2], &D=coefs[3], &E=coefs[4];
A = 1;
VectorR3 viewPosRel;
viewPosRel = viewDir;
viewPosRel *= moveFwdDist; // Move forward distance moveFwdDist
viewPosRel += viewPos;
viewPosRel -= Center; // Position relative to center
double udotp = (viewDir^viewPosRel);
B = 4.0*udotp;
double MSq = Square(MajorRadius);
double mSq = Square(MinorRadius);
double RadiiSqSum = MSq + mSq;
double ucdotp = (AxisC^viewPosRel);
double ucdotu = (AxisC^viewDir);
double pSq = (viewPosRel^viewPosRel);
C = 4.0*udotp*udotp + 2.0*pSq - 2.0*RadiiSqSum + 4.0*MSq*ucdotu*ucdotu;
D = 4.0 * ( (pSq-RadiiSqSum)*udotp + 2.0*MSq*ucdotp*ucdotu );
E = (pSq - 2.0*RadiiSqSum)*pSq + 4.0*MSq*ucdotp*ucdotp + Square(MSq-mSq);
int numRoots = PolySolveReal( 4, coefs, roots );
for ( int i=0; i<numRoots; i++ ) {
roots[i] += moveFwdDist; // Restate as distance from viewPos
if ( roots[i] >= maxDistance ) {
return false;
}
if ( roots[i]>0.0 ) {
// Return this visible point
*intersectDistance = roots[i];
VectorR3 Point = viewDir;
Point *= roots[i];
Point += viewPos;
returnedPoint.SetPosition(Point); // Intersection position (not relative to center)
if ( i & 0x01 ) {
returnedPoint.SetBackFace(); // Orientation
returnedPoint.SetMaterial( *InnerMaterial ); // Material
}
else {
returnedPoint.SetFrontFace(); // Orientation
returnedPoint.SetMaterial( *OuterMaterial ); // Material
}
// Outward normal
Point -= Center; // Now its the relative point
double xCoord = Point^AxisA; // forward axis
double yCoord = Point^AxisB; // rightward axis
double zCoord = Point^AxisC; // upward axis
VectorR3 outN = AxisC;
outN *= -zCoord;
outN += Point; // Project point down to plane of torus center
double outNnorm = outN.Norm();
outN *= MajorRadius/(-outNnorm); // Negative point projected to center path of torus
outN += Point; // Displacement of point from center path of torus
outN /= MinorRadius; // Should be outward unit vector
outN.ReNormalize(); // Fix roundoff error problems
returnedPoint.SetNormal(outN); // Outward normal
// u - v coordinates
double u = atan2( yCoord, xCoord );
u = u*PI2inv+0.5;
double bVal = outNnorm-MajorRadius;
double v = atan2( zCoord, bVal );
v = v*PI2inv+0.5;
returnedPoint.SetUV(u , v);
returnedPoint.SetFaceNumber( 0 );
return true;
}
}
return false;
}
