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); }