#include <pfcave.h>
#include "grabberDCS.h"



pfType *grabberDCS::classType_ = NULL; 

void grabberDCS::init(void)
{
 if (classType_ == NULL)
	{
        pfDCS::init();
        classType_ =  new pfType(pfDCS::getClassType(), "grabberDCS");
	}
}



grabberDCS::grabberDCS(void)
{
 setType(classType_);  /* set the Performer type of this instance */
 grabbed_ = 0;
 grabMat_.makeIdent();
}


int grabberDCS::isGrabbed(void)
{
 return grabbed_;
}


void grabberDCS::grab(void)
{
 if (!grabbed_)
	{
	pfMatrix invmat;
	GetInverseWandMatrix(invmat);
	getMat(grabMat_);
	grabMat_ *= invmat;
	grabbed_ = 1;
	}
}


void grabberDCS::release(void)
{
 grabbed_ = 0;
}


int grabberDCS::app(pfTraverser *trav)
{
 if (grabbed_)
	{
	pfMatrix sensMat, newMat;
	GetWandMatrix(sensMat);
	newMat = grabMat_ * sensMat;
	setMat(newMat);
	}
 return pfDCS::app(trav);
}

int grabberDCS::needsApp(void)
{
 return TRUE;
}


void grabberDCS::GetWandMatrix(pfMatrix& m)
{
 float or[3],pos[3];
 CAVEGetPosition(CAVE_WAND_NAV,pos); 
 CAVEGetOrientation(CAVE_WAND_NAV,or);
 m.makeTrans(pos[0],pos[1],pos[2]);
 m.preRot(or[2],0.0,0.0,1.0,m);
 m.preRot(or[0],1.0,0.0,0.0,m);
 m.preRot(or[1],0.0,1.0,0.0,m);
}


void grabberDCS::GetInverseWandMatrix(pfMatrix& m)
{
 float or[3],pos[3];
 CAVEGetPosition(CAVE_WAND_NAV,pos); 
 CAVEGetOrientation(CAVE_WAND_NAV,or);
 m.makeRot(-or[1],0.0,1.0,0.0);
 m.preRot(-or[0],1.0,0.0,0.0,m);
 m.preRot(-or[2],0.0,0.0,1.0,m);
 m.preTrans(-pos[0],-pos[1],-pos[2],m);
}