void hduQuaternion::fromRotationMatrix(const hduMatrix& rotMat)
{
double r[3][3];
r[0][0] = rotMat.get(0,0);
r[1][0] = rotMat.get(1,0);
r[2][0] = rotMat.get(2,0);
r[0][1] = rotMat.get(0,1);
r[1][1] = rotMat.get(1,1);
r[2][1] = rotMat.get(2,1);
r[0][2] = rotMat.get(0,2);
r[1][2] = rotMat.get(1,2);
r[2][2] = rotMat.get(2,2);
fromRotationMatrix(r);
}
//
// The Graphics Callback runs in the application "client thread" (qhStart) and sets the transformations
// for the Red Sphere and Green Line of the Cursor. Also, this callback sets the WorldToDevice matrix
// for use in the ServoLoopCallback.
//
void GraphicsCallback(void)
{
QHGLUT* localDisplayObject = QHGLUT::searchWindow("Coulomb Field Demo");//Get a Pointer to the display object
Cursor* localDeviceCursor = Cursor::searchCursor("devCursor");//Get a pointer to the cursor
Cylinder* localForceArrow = Cylinder::searchCylinder("forceArrow");//get a pointer to the cylinder
Cone* localForceArrowTip = Cone::searchCone("forceArrowTip");//get a pointer to the cylinder
Sphere* localCursorSphere = Sphere::searchSphere("cursorSphere");//get a pointer top the Sphere
if( localDisplayObject == NULL || localDeviceCursor == NULL || localForceArrow == NULL || localCursorSphere == NULL)
return;
hduMatrix CylinderTransform;//Transformation for the Cylinder. This transform makes it point toward the Model
hduVector3Dd localCursorPosition;
hduVector3Dd DirectionVecX;
hduVector3Dd PointOnPlane;
hduVector3Dd DirectionVecY;
hduVector3Dd DirectionVecZ;
//Compute the world to device transform
WorldToDevice = localDisplayObject->getWorldToDeviceTransform();
// Set transform for Red Sphere
localCursorPosition = localDeviceCursor->getPosition();//Get the local cursor position in World Space
hduVector3Dd localCursorSpherePos = localCursorSphere->getTranslation();
localCursorSphere->setTranslation(-localCursorSpherePos);
localCursorSphere->setTranslation(localCursorPosition);//Set the position of the Sphere the same as the cursor
////////////////////////////////////////////////////////////////////////////////////////////
//Code to calculate the transform of the green cylinder to point along the force direction
////////////////////////////////////////////////////////////////////////////////////////////
hduMatrix DeviceToWorld = WorldToDevice.getInverse();
HDdouble ForceMagnitude = forceVec.magnitude();
DeviceToWorld[3][0] = 0.0;
DeviceToWorld[3][1] = 0.0;
DeviceToWorld[3][2] = 0.0;
DirectionVecX = forceVec * DeviceToWorld;
DirectionVecX.normalize();
PointOnPlane.set(0.0,0.0,(DirectionVecX[0]*localCursorPosition[0] + DirectionVecX[1]*localCursorPosition[1] + DirectionVecX[2]*localCursorPosition[2])/DirectionVecX[2]);
DirectionVecY = PointOnPlane - localCursorPosition;
DirectionVecY.normalize();
DirectionVecZ = -DirectionVecY.crossProduct(DirectionVecX);
CylinderTransform[0][0] = DirectionVecZ[0]; CylinderTransform[0][1] = DirectionVecZ[1]; CylinderTransform[0][2] = DirectionVecZ[2]; CylinderTransform[0][3] = 0.0;
CylinderTransform[1][0] = DirectionVecX[0]; CylinderTransform[1][1] = DirectionVecX[1]; CylinderTransform[1][2] = DirectionVecX[2]; CylinderTransform[1][3] = 0.0;
CylinderTransform[2][0] = DirectionVecY[0]; CylinderTransform[2][1] = DirectionVecY[1]; CylinderTransform[2][2] = DirectionVecY[2]; CylinderTransform[2][3] = 0.0;
CylinderTransform[3][0] = 0.0 ; CylinderTransform[3][1] = 0.0 ; CylinderTransform[3][2] = 0.0 ; CylinderTransform[3][3] = 1.0;
CylinderTransform = CylinderTransform * hduMatrix::createTranslation(localCursorPosition[0], localCursorPosition[1], localCursorPosition[2]);
localForceArrow->update(chargeRadius/4, ForceMagnitude*50, 15);
localForceArrow->setTranslation(localCursorPosition);
localForceArrow->setTransform(CylinderTransform);
hduMatrix ConeTransform = CylinderTransform * hduMatrix::createTranslation(DirectionVecX[0]
* ForceMagnitude*50,DirectionVecX[1] * ForceMagnitude*50,DirectionVecX[2] * ForceMagnitude*50 );
localForceArrowTip->setTransform(ConeTransform);
/////////////////////////////////////////////
}
//
// applyCameraAngle()
// Rotation to match omni frame with camera view.
//
// Precondition: pos is a position vector in the Omni frame.
// Postcondition: pos is a position vector in the camera view frame.
//
void applyCameraAngle(hduMatrix &xform){
double c1 = cos(-th1*DEGREES), c2 = cos(-th2*DEGREES), c3 = cos(th3*DEGREES);
double s1 = sin(-th1*DEGREES), s2 = sin(-th2*DEGREES), s3 = sin(th3*DEGREES);
hduMatrix R( c1*c3+s1*s2*s3 , c3*s1*s2-c1*s3 , c2*s1 , 0,
c2*s3 , c2*c3 , -1*s2 , 0,
c1*s2*s3-c3*s1 , c1*c3*s2+s1*s3 , c1*c2 , 0,
0 , 0 , 0 , 1);
xform = xform.multRight(R);
// Print rotation matrix
//cout << R[0][0] << "\t" << R[0][1] << "\t" << R[0][2] << "\n";
//cout << R[1][0] << "\t" << R[1][1] << "\t" << R[1][2] << "\n";
//cout << R[2][0] << "\t" << R[2][1] << "\t" << R[2][2] << "\n" <<"\n";
// Matrix-vector multiply R.p
//for (int i = 0; i<4; i++){
// temp[i]=0;
// for (int j=0; j<4; j++){
// temp[i] += R[i][j]*pos[3][j];
// }
//}
//// Set output
//for (int i = 0; i<3; i++){
// pos[3][i] = temp[i];
//}
}
void omni2ITPTransform(hduMatrix & xform)
{
const hduMatrix omni2ITP_x( 0 , 1 , 0 , 0 ,
0 , 0 , -1 , 0 ,
-1 , 0 , 0 , 0 ,
0 , 0 , 0 , 1 );
const hduMatrix omni2ITP( 0 , 0 , -1 , 0 ,
1 , 0 , 0 , 0 ,
0 , -1 , 0 , 0 ,
0 , 0 , 0 , 1 );
xform.multRight(omni2ITP);
}
void omni2ITPTransform(hduMatrix & xform , int devID)
{
if (devID == OMNI1){
const hduMatrix omni2ITP( 0 , 1 , 0 , 0 ,
0 , 0 , -1 , 0 ,
-1 , 0 , 0 , 0 ,
0 , 0 , 0 , 1 );
xform.multRight(omni2ITP);
}
if (devID == OMNI2){
const hduMatrix omni2ITP( 0 , 1 , 0 , 0 ,
0 , 0 , -1 , 0 ,
-1 , 0 , 0 , 0 ,
0 , 0 , 0 , 1 );
xform.multRight(omni2ITP);
}
/*const hduMatrix omni2ITP_x( 0 , 0 , -1 , 0 ,
1 , 0 , 0 , 0 ,
0 , -1 , 0 , 0 ,
0 , 0 , 0 , 1 );*/
}
//////////////////////////////////////////////////////////////////////////////
// transformFrustum -
// Transform srcFrustum by mat, result in dstFrustum.
//
//////////////////////////////////////////////////////////////////////////////
void transformFrustum(
const hduMatrix& mat,
const hduVector3Dd* srcFrustum,
hduVector3Dd* dstFrustum
)
{
for (int i = 0; i < 8; ++i)
{
hduVector3Dd dst;
mat.multVecMatrix(srcFrustum[i], dst);
dstFrustum[i][0] = dst[0];
dstFrustum[i][1] = dst[1];
dstFrustum[i][2] = dst[2];
}
}
/*******************************************************************************
GLUT callback for mouse motion, which is used for controlling the view
rotation and scaling.
*******************************************************************************/
void glutMotion(int x, int y)
{
if (gIsRotatingCamera)
{
static const double kTrackBallRadius = 0.8;
hduVector3Dd lastPos;
lastPos[0] = gLastMouseX * 2.0 / gWindowWidth - 1.0;
lastPos[1] = (gWindowHeight - gLastMouseY) * 2.0 / gWindowHeight - 1.0;
lastPos[2] = projectToTrackball(kTrackBallRadius, lastPos[0], lastPos[1]);
hduVector3Dd currPos;
currPos[0] = x * 2.0 / gWindowWidth - 1.0;
currPos[1] = (gWindowHeight - y) * 2.0 / gWindowHeight - 1.0;
currPos[2] = projectToTrackball(kTrackBallRadius, currPos[0], currPos[1]);
currPos.normalize();
lastPos.normalize();
hduVector3Dd rotateVec = lastPos.crossProduct(currPos);
double rotateAngle = asin(rotateVec.magnitude());
if (!hduIsEqual(rotateAngle, 0.0, DBL_EPSILON))
{
hduMatrix deltaRotation = hduMatrix::createRotation(
rotateVec, rotateAngle);
gCameraRotation.multRight(deltaRotation);
updateCamera();
}
}
else if (gIsScalingCamera)
{
float y1 = gWindowHeight - gLastMouseY;
float y2 = gWindowHeight - y;
gCameraScale *= 1 + (y1 - y2) / gWindowHeight;
updateCamera();
}
gLastMouseX = x;
gLastMouseY = y;
}
/***************************************************************************************
Servo loop thread callback. Computes a force effect. This callback defines the motion
of the purple skull and calculates the force based on the "real-time" Proxy position
in Device space.
****************************************************************************************/
void HLCALLBACK computeForceCB(HDdouble force[3], HLcache *cache, void *userdata)
{
DataTransportClass *localdataObject = (DataTransportClass *) userdata;//Typecast the pointer passed in appropriately
hduVector3Dd skullPositionDS;//Position of the skull (Moving sphere) in Device Space.
hduVector3Dd proxyPosition;//Position of the proxy in device space
HDdouble instRate = 0.0;
HDdouble deltaT = 0.0;
static float counter = 0.0;
float degInRad = 0.0;
static int counter1 = 0;
// Get the time delta since the last update.
hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &instRate);
deltaT = 1.0 / instRate;
counter+=deltaT;
degInRad = counter*20*3.14159/180;
hduVector3Dd ModelPos = localdataObject->Model->getTranslation();
localdataObject->Model->setTranslation(-ModelPos);
localdataObject->Model->setTranslation(cos(degInRad)*64.0, sin(degInRad)*64.0,5.0);//Move the skull aroubnd in a circle
WorldToDevice.multVecMatrix(localdataObject->Model->getTranslation(),skullPositionDS);//Convert the position of the sphere from world space to device space
hlCacheGetDoublev(cache, HL_PROXY_POSITION, proxyPosition);//Get the position of the proxy in Device Coordinates (All HL commands in the servo loop callback fetch values in device coordinates)
forceVec = forceField(proxyPosition, skullPositionDS, 40.0, 5.0);//Calculate the force
counter1++;
if(counter1>2000)//Make the force start after 2 seconds of program start. This is because the servo loop thread executes before the graphics thread.
//Hence global variables set in the graphics thread will not be valid for sometime in the begining og the program
{
force[0] = forceVec[0];
force[1] = forceVec[1];
force[2] = forceVec[2];
counter1 = 2001;
}
else
{
force[0] = 0.0;
force[1] = 0.0;
force[2] = 0.0;
}
}
void hduQuaternion::toRotationMatrix(hduMatrix& rotMat) const
{
double r[3][3];
toRotationMatrix(r);
rotMat.set(0, 0, r[0][0]);
rotMat.set(0, 1, r[0][1]);
rotMat.set(0, 2, r[0][2]);
rotMat.set(0, 3, 0);
rotMat.set(1, 0, r[1][0]);
rotMat.set(1, 1, r[1][1]);
rotMat.set(1, 2, r[1][2]);
rotMat.set(1, 3, 0);
rotMat.set(2, 0, r[2][0]);
rotMat.set(2, 1, r[2][1]);
rotMat.set(2, 2, r[2][2]);
rotMat.set(2, 3, 0);
rotMat.set(3, 0, 0);
rotMat.set(3, 1, 0);
rotMat.set(3, 2, 0);
rotMat.set(3, 3, 1);
}