コード例 #1
0
ファイル: hduQuaternion.cpp プロジェクト: gzmk/phantom_code
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);
}
コード例 #2
0
ファイル: main.cpp プロジェクト: danepowell/openhaptics
//
// 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);
	/////////////////////////////////////////////
}
コード例 #3
0
//
// 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];
	//}
}
コード例 #4
0
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);
}
コード例 #5
0
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	);*/

}
コード例 #6
0
//////////////////////////////////////////////////////////////////////////////
//  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];
    }
}
コード例 #7
0
ファイル: main.cpp プロジェクト: oliriley/PhantomController
/*******************************************************************************
 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;
}
コード例 #8
0
ファイル: main.cpp プロジェクト: danepowell/openhaptics
/***************************************************************************************
 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;
    }
}
コード例 #9
0
ファイル: hduQuaternion.cpp プロジェクト: gzmk/phantom_code
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);
}