float difference(const Descriptor* desc1, const Descriptor* desc2, Descriptor& desc, float (*diff) (float, float) )
{
// assuming desc is valid
if( desc2 == 0){
desc.clear();
desc.m_na = desc1->size_a();
desc.m_nz = desc1->size_z();
desc.m_nr = desc1->size_r();
Matrixf *pm = new Matrixf( * desc1->getPage(0) );
pm->clone();
desc.m_vHistPtr.push_back(pm);
return 0.0f;
}
else if( desc1 == desc2 ){
desc.clear();
desc.m_na = desc1->size_a();
desc.m_nz = desc1->size_z();
desc.m_nr = desc1->size_r();
Matrixf *pm = new Matrixf( desc.m_na * desc.m_nz, desc.m_nr );
pm->setAllZeros();
desc.m_vHistPtr.push_back(pm);
return 0.0f;
}
else
return difference(*desc1, *desc2, desc, diff);
}
Descriptor::Descriptor( const Neuron& neuron, const SphericalMesh& mesh)
: m_fSumWeight(0)
{
m_na = mesh.numAzimuth();
m_nz = mesh.numZenith();
m_nr = mesh.numRadius();
vector<Vector3> points;
vector<float> weights;
gatherPoints( neuron, points, weights );
Matrixf* pHist = new Matrixf(height(), width());
pHist->setAllZeros();
for( size_t i = 0; i < points.size(); ++i){
vector<Index3D> index;
mesh.get_index( points[i].spherical(), index );
for( size_t j = 0; j < index.size(); ++j){
float weight = 1.0f;// weights[i];
assert(weight);
(*pHist)( index[j].aidx * m_nz +
index[j].zidx, index[j].ridx) += weight;
m_fSumWeight += weight;
}
}
m_vHistPtr.push_back( pHist);
vFlip(true);
hFlip(true);
rotate(0); // rotate the original histogram
rotate(1); // rotate the virtically flipped one
rotate(2); // rotate the horizontically flipped one
}
/***************************************************************
* Function: setCenterPos()
***************************************************************/
void CAVEGroupReference::setCenterPos(const Vec3 ¢er)
{
mCenter = center;
Matrixf transMat;
transMat.makeTranslate(mCenter);
mMatrixTrans->setMatrix(transMat);
}
/***************************************************************
* Function: setCenterPos()
***************************************************************/
void CAVEGroupReferencePlane::setCenterPos(const osg::Vec3 ¢er)
{
/* snap 'center' vector with respect to mUnitGridSize */
Vec3 centerRounded;
float snapUnitX, snapUnitY, snapUnitZ;
snapUnitX = snapUnitY = snapUnitZ = mUnitGridSize;
if (center.x() < 0) snapUnitX = -mUnitGridSize;
if (center.y() < 0) snapUnitY = -mUnitGridSize;
if (center.z() < 0) snapUnitZ = -mUnitGridSize;
int xSeg = (int)(abs((int)((center.x() + 0.5 * snapUnitX) / mUnitGridSize)));
int ySeg = (int)(abs((int)((center.y() + 0.5 * snapUnitY) / mUnitGridSize)));
int zSeg = (int)(abs((int)((center.z() + 0.5 * snapUnitZ) / mUnitGridSize)));
centerRounded.x() = xSeg * snapUnitX;
centerRounded.y() = ySeg * snapUnitY;
centerRounded.z() = zSeg * snapUnitZ;
/* set highlights of either XZ plane or YZ plane */
Vec3 offset = centerRounded - mCenter;
float offx = offset.x() * offset.x();
float offy = offset.y() * offset.y();
if (offx > offy) mYZPlaneGeode->setAlpha(1.0f);
else mYZPlaneGeode->setAlpha(0.4f);
if (offx < offy) mXZPlaneGeode->setAlpha(1.0f);
else mXZPlaneGeode->setAlpha(0.4f);
mCenter = centerRounded;
Matrixf transMat;
transMat.makeTranslate(mCenter);
mMatrixTrans->setMatrix(transMat);
}
void Tracker::replayTracker(vector<Matrixu> &vid, string statesfile, string outputvid, uint R, uint G, uint B)
{
Matrixf states;
states.DLMRead(statesfile.c_str());
Matrixu colorframe;
// save video file
CvVideoWriter* w = NULL;
if( ! outputvid.empty() ){
w = cvCreateVideoWriter( outputvid.c_str(), CV_FOURCC('I','Y','U','V'), 15, cvSize(vid[0].cols(), vid[0].rows()), 3 );
if( w==NULL ) abortError(__LINE__,__FILE__,"Error opening video file for output");
}
for( uint k=0; k<vid.size(); k++ )
{
vid[k].conv2RGB(colorframe);
colorframe.drawRect(states(k,2),states(k,3),states(k,0),states(k,1),1,0,2,R,G,B);
colorframe.drawText(("#"+int2str(k,3)).c_str(),1,25,255,255,0);
colorframe._keepIpl=true;
colorframe.display(1,2);
cvWaitKey(1);
if( w != NULL )
cvWriteFrame( w, colorframe.getIpl() );
colorframe._keepIpl=false; colorframe.freeIpl();
}
// clean up
if( w != NULL )
cvReleaseVideoWriter( &w );
}
float difference(const Descriptor & desc1, const Descriptor & desc2, Descriptor& desc, float (*diff) (float, float) ){
size_t idx = 0;
float mindiff = inf;
size_t height = desc1.height();
size_t width = desc1.width();
float sum;
for( size_t k = 0; k < desc1.page(); ++k)
{ // k: page # of flipped/rotated descriptors
sum = 0.0f;
for( size_t i = 0; i < height; ++i){ //i, iterates all zenith and azimuth angle bins
for( size_t j = 0; j < width; ++j) //j iterates all radius bins
sum += diff( desc1(i, j, 0), desc2(i,j, k));
}
if( mindiff > sum){
idx = k;
mindiff = sum;
}
}
desc.clear();
Matrixf *pm = new Matrixf( *desc2.getPage( idx ) );
pm->clone();
*pm -= *desc1.getPage(0);
// pm->getAbsoluteValue();
desc.m_vHistPtr.push_back( pm );
desc.m_na = desc1.size_a();
desc.m_nz = desc1.size_z();
desc.m_nr = desc1.size_r();
return mindiff / (desc1.sumWeight() + desc2.sumWeight());
}
/***************************************************************
* Function: applyEditorInfo()
***************************************************************/
void CAVEGroupEditGeodeWireframe::applyEditorInfo(CAVEGeodeShape::EditorInfo **infoPtr)
{
/* reset root offset only when the shape is moved */
if ((*infoPtr)->getTypeMasking() == CAVEGeodeShape::EditorInfo::MOVE)
{
/* apply translations on 'mAccRootMat' */
const Vec3 offset = (*infoPtr)->getMoveOffset();
Matrixd transMat;
transMat.makeTranslate(offset);
mAccRootMat = mAccRootMat * transMat;
}
else if ((*infoPtr)->getTypeMasking() == CAVEGeodeShape::EditorInfo::ROTATE)
{
/* apply rotations on 'mAccRootMat' */
const float angle = (*infoPtr)->getRotateAngle();
const Vec3 axis = (*infoPtr)->getRotateAxis();
Matrixf rotateMat;
rotateMat.makeRotate(angle, axis);
mAccRootMat = mAccRootMat * rotateMat;
}
else if ((*infoPtr)->getTypeMasking() == CAVEGeodeShape::EditorInfo::SCALE)
{
const Vec3f scaleVect = (*infoPtr)->getScaleVect();
const Vec3f scaleCenter = (*infoPtr)->getScaleCenter();
Matrixd scalingMat, transMat, revTransMat;
scalingMat.makeScale(scaleVect);
transMat.makeTranslate(scaleCenter);
revTransMat.makeTranslate(-scaleCenter);
mAccRootMat = mAccRootMat * scalingMat;
}
}
/**
* Extract a row from a matrix
* @param matrix The matrix that we are extracting the row for
* @param rowNumber The number of the row that we are working
* @return The matrix row that we are retrieving
*/
Matrixf GetMatrixRow(Matrixf & matrix, int rowNumber)
{
Matrixf result(matrix.ncols(), 1);
for (int column = 0; column<matrix.ncols(); column++)
{
float value = matrix.get(rowNumber, column);
result.set(column, 0, value);
}
return result;
}
/***************************************************************
* Function: preFrame()
***************************************************************/
void Maze2::preFrame()
{
/* get pointer position in world space */
Matrixf invBaseMat = PluginHelper::getWorldToObjectTransform();
Matrixf viewMat = PluginHelper::getHeadMat(0);
Matrixf xformMat = PluginHelper::getObjectMatrix();
/* get viewer's position in world space */
Vec3 viewOrg = viewMat.getTrans() * invBaseMat;
Vec3 viewPos = Vec3(0.0, 1.0, 0.0) * viewMat * invBaseMat;
Vec3 viewDir = viewPos - viewOrg;
viewDir.normalize();
mAudioConfigHandler->updatePoses(viewDir, viewPos);
mNaviHandler->updateNaviStates(Navigation::instance()->getScale(), viewDir, viewPos);
// mNaviHandler->updateButtonStates();
mNaviHandler->updateXformMat();
/* ECGClient: Master-Slave operations: Read current time for update */
double frameDuration;
if(ComController::instance()->isMaster())
{
frameDuration = PluginHelper::getLastFrameDuration();
((double*)mClkBuf)[0] = frameDuration;
ComController::instance()->sendSlaves((char*) &mClkBuf, sizeof(mClkBuf));
} else {
ComController::instance()->readMaster((char*) &mClkBuf, sizeof(mClkBuf));
frameDuration = ((double*)mClkBuf)[0];
}
mECGClient->update(viewMat, invBaseMat, xformMat, frameDuration);
/*
cerr << "viewPos = " << viewPos.x() << " " << viewPos.y() << " " << viewPos.z() << endl;
cerr << "scale = " << PluginHelper::getObjectScale() << endl;
cerr << "invBaseMat: " << endl;
cerr << invBaseMat(0, 0) << " " << invBaseMat(0, 1) << " " << invBaseMat(0, 2) << " " << invBaseMat(0, 3) << " " << endl;
cerr << invBaseMat(1, 0) << " " << invBaseMat(1, 1) << " " << invBaseMat(1, 2) << " " << invBaseMat(1, 3) << " " << endl;
cerr << invBaseMat(2, 0) << " " << invBaseMat(2, 1) << " " << invBaseMat(2, 2) << " " << invBaseMat(2, 3) << " " << endl;
cerr << invBaseMat(3, 0) << " " << invBaseMat(3, 1) << " " << invBaseMat(3, 2) << " " << invBaseMat(3, 3) << " " << endl;
cerr << "xformMat: " << endl;
cerr << xformMat(0, 0) << " " << xformMat(0, 1) << " " << xformMat(0, 2) << " " << xformMat(0, 3) << " " << endl;
cerr << xformMat(1, 0) << " " << xformMat(1, 1) << " " << xformMat(1, 2) << " " << xformMat(1, 3) << " " << endl;
cerr << xformMat(2, 0) << " " << xformMat(2, 1) << " " << xformMat(2, 2) << " " << xformMat(2, 3) << " " << endl;
cerr << xformMat(3, 0) << " " << xformMat(3, 1) << " " << xformMat(3, 2) << " " << xformMat(3, 3) << " " << endl;
cerr << "viewMat: " << endl;
cerr << viewMat(0, 0) << " " << viewMat(0, 1) << " " << viewMat(0, 2) << " " << viewMat(0, 3) << " " << endl;
cerr << viewMat(1, 0) << " " << viewMat(1, 1) << " " << viewMat(1, 2) << " " << viewMat(1, 3) << " " << endl;
cerr << viewMat(2, 0) << " " << viewMat(2, 1) << " " << viewMat(2, 2) << " " << viewMat(2, 3) << " " << endl;
cerr << viewMat(3, 0) << " " << viewMat(3, 1) << " " << viewMat(3, 2) << " " << viewMat(3, 3) << " " << endl;
cerr << endl;
*/
}
Matrixf XSAdjoint::Solve(const Matrixf& A, const Matrixf& Y) const
{
if (A.rows() == A.columns())
{
return A.inverse() * Y;
}
else
{
std::cerr
<< "Error!! Code: Matrixf aries::utility::XSAdjoint::Solve(const Matrixf&, const Matrixf&)" << std::endl
<< " Note: Nonsquare matrix cannot bo solved." << std::endl;
exit(-1);
}
}
void Descriptor::hFlip(bool duplicate)
{
Matrixf *pm = m_vHistPtr[0];
if( duplicate ){
pm = new Matrixf( *m_vHistPtr[0] );
pm->clone();
m_vHistPtr.push_back(pm);
}
for( size_t z = 0; z < size_z(); ++z){
for( size_t a = 0; a < size_a()/2; ++a){
pm->swaprows(a * size_z() + z, ( size_a() - a -1 )* size_z() + z);
}
}
}
void Descriptor::rotate( size_t i)
{
// we will rotate and duplicate the i-th histogram. i generally is 0 ~ 2;
Matrixf* pm = new Matrixf(*m_vHistPtr[i] );
pm->clone();
pm->rshrows( size_z() );
m_vHistPtr.push_back( pm ); //k = 1
for( size_t k = 2; k < size_a(); ++k ){
Matrixf* pm = new Matrixf( *m_vHistPtr.back() );
pm->clone();
pm->rshrows( size_z() );
m_vHistPtr.push_back( pm );
}
}
/***************************************************************
* Function: preFrame()
***************************************************************/
void EOGCalibration::preFrame()
{
Matrixf invBaseMat = PluginHelper::getWorldToObjectTransform();
Matrixf viewMat = PluginHelper::getHeadMat(0);
Matrixf xformMat = PluginHelper::getObjectMatrix();
/* get viewer's position in world space */
Vec3 viewOrg = viewMat.getTrans() * invBaseMat;
Vec3 viewPos = Vec3(0.0, 1.0, 0.0) * viewMat * invBaseMat;
Vec3 viewDir = viewPos - viewOrg;
viewDir.normalize();
//mAudioConfigHandler->updatePoses(viewDir, viewPos);
//mNaviHandler->updateNaviStates(Navigation::instance()->getScale(), viewDir, viewPos);
//mNaviHandler->updateButtonStates();
//mNaviHandler->updateXformMat();
/* ECGClient: Master-Slave operations: Read current time for update */
double frameDuration;
if(ComController::instance()->isMaster())
{
frameDuration = PluginHelper::getLastFrameDuration();
((double*)mClkBuf)[0] = frameDuration;
cvr::ComController::instance()->sendSlaves((char*) &mClkBuf, sizeof(mClkBuf));
}
else
{
cvr::ComController::instance()->readMaster((char*) &mClkBuf, sizeof(mClkBuf));
frameDuration = ((double*)mClkBuf)[0];
}
/* update inverse base matrix for re-aligning calibration field */
mInvBaseMat = viewMat * invBaseMat;
mCaliController->updateViewMat(viewMat);
/* DEBUGGING OUTPUT: Head position and orientations in CAVE space
Vec3 rightVec = Vec3(viewMat(0, 0), viewMat(0, 1), viewMat(0, 2));
Vec3 frontVec= Vec3(viewMat(1, 0), viewMat(1, 1), viewMat(1, 2));
Vec3 upVec = Vec3(viewMat(2, 0), viewMat(2, 1), viewMat(2, 2));
Vec3 headPos = Vec3(viewMat(3, 0), viewMat(3, 1), viewMat(3, 2)) / 1000.f;
cerr << "Right vector = " << rightVec.x() << " " << rightVec.y() << " " << rightVec.z() << endl;
cerr << "Front vector = " << frontVec.x() << " " << frontVec.y() << " " << frontVec.z() << endl;
cerr << "Up vector = " << upVec.x() << " " << upVec.y() << " " << upVec.z() << endl;
cerr << "Head position = " << headPos.x() << " " << headPos.y() << " " << headPos.z() << endl;
// cerr << "phi = " << phi << " theta = " << theta << " rad = " << rad << endl;
cerr << endl;
DEBUGGING OUTPUT: Viewer positions and orientations in world space
Vec3 rightVec = Vec3(xformMat(0, 0), xformMat(0, 1), xformMat(0, 2));
Vec3 frontVec= Vec3(xformMat(1, 0), xformMat(1, 1), xformMat(1, 2));
Vec3 upVec = Vec3(xformMat(2, 0), xformMat(2, 1), xformMat(2, 2));
Vec3 headPos = Vec3(xformMat(0, 3), xformMat(1, 3), xformMat(2, 3)) / 1000.f * (-1);
cerr << "Right vector = " << rightVec.x() << " " << rightVec.y() << " " << rightVec.z() << endl;
cerr << "Front vector = " << frontVec.x() << " " << frontVec.y() << " " << frontVec.z() << endl;
cerr << "Up vector = " << upVec.x() << " " << upVec.y() << " " << upVec.z() << endl;
cerr << "Head position = " << headPos.x() << " " << headPos.y() << " " << headPos.z() << endl;
cerr << endl; */
float phi, theta, rad;
if (mCaliController->isCaliBallVisible())
{
mCaliController->updateCaliTime(frameDuration);
mCaliController->updateCaliBallPos(phi, theta, rad);
}
if (mCaliController->isPlaybackBallVisible())
{
mCaliController->updatePlaybackTime(frameDuration);
mCaliController->updatePlaybackBallPos();
playbackTimerLabel->setText(mCaliController->getPlaybackTimeLabel());
}
/* create client message and send to data server */
if (mFlagConnected && mFlagMaster && mCaliController->isCalibrationStarted() && mCaliController->isCaliBallVisible())
{
Vec3 headPos = Vec3(viewMat(3, 0), viewMat(3, 1), viewMat(3, 2)) / 1000.f;
struct MSGClient msg(viewMat(0, 0), viewMat(0, 1), viewMat(0, 2), viewMat(1, 0), viewMat(1, 1), viewMat(1, 2),
viewMat(2, 0), viewMat(2, 1), viewMat(2, 2), headPos.x(), headPos.y(), headPos.z(),
phi, theta, rad);
int nBytes = send(mSockfd, &msg, sizeof (MSGClient), 0);
if (nBytes != sizeof (MSGClient))
{
std::cerr << "EOGCalibration Warning: Lost data transmission. nBytes = " << nBytes << std::endl;
}
}
}
/***************************************************************
* Function: acceptCAVEGeodeShape()
*
* Compared with original coordinates data in 'CAVEGeodeShape',
* generated coordinates in 'CAVEGeodeIconSurface' is eaxctly the
* the same as those appeared in 'CAVEGeodeShape'. The scaling and
* translation to 'gIconCenter' effects are impletemented by its
* acendent 'PositionAltitudeTransform' object.
*
***************************************************************/
void CAVEGroupIconSurface::acceptCAVEGeodeShape(CAVEGeodeShape *shapeGeode, CAVEGeodeShape *shapeGeodeRef)
{
mCAVEGeodeShapeOriginPtr = shapeGeode;
CAVEGeometryVector &orgGeomVector = shapeGeode->getCAVEGeometryVector();
CAVEGeometryVector &refGeomVector = shapeGeodeRef->getCAVEGeometryVector();
int nGeoms = orgGeomVector.size();
if (nGeoms <= 0) return;
/* re-generate vertex coordinate list, keep normal and texcoords the same from 'CAGEGeodeShape' */
mSurfVertexArray = new Vec3Array;
mSurfNormalArray = new Vec3Array;
mSurfUDirArray = new Vec3Array;
mSurfVDirArray = new Vec3Array;
mSurfTexcoordArray = new Vec2Array;
Vec3Array* geodeVertexArray = shapeGeode->mVertexArray;
Vec3Array* geodeNormalArray = shapeGeode->mNormalArray;
Vec3Array* geodeUDirArray = shapeGeode->mUDirArray;
Vec3Array* geodeVDirArray = shapeGeode->mVDirArray;
Vec2Array* geodeTexcoordArray = shapeGeode->mTexcoordArray;
Vec3 *geodeVertexDataPtr, *geodeNormalDataPtr, *geodeUDirDataPtr, *geodeVDirDataPtr;
Vec2 *geodeTexcoordDataPtr;
/* check the valid status of all data field from 'CAVEGeodeShape' */
if (geodeVertexArray->getType() == Array::Vec3ArrayType)
geodeVertexDataPtr = (Vec3*) (geodeVertexArray->getDataPointer());
else return;
if (geodeNormalArray->getType() == Array::Vec3ArrayType)
geodeNormalDataPtr = (Vec3*) (geodeNormalArray->getDataPointer());
else return;
if (geodeUDirArray->getType() == Array::Vec3ArrayType)
geodeUDirDataPtr = (Vec3*) (geodeUDirArray->getDataPointer());
else return;
if (geodeVDirArray->getType() == Array::Vec3ArrayType)
geodeVDirDataPtr = (Vec3*) (geodeVDirArray->getDataPointer());
else return;
if (geodeTexcoordArray->getType() == Array::Vec2ArrayType)
geodeTexcoordDataPtr = (Vec2*) (geodeTexcoordArray->getDataPointer());
else return;
/* convert vertex coordinates from CAVEGeodeShape space to CAVEGeodeIcon space */
int nVerts = shapeGeode->mNumVertices;
for (int i = 0; i < nVerts; i++) mSurfVertexArray->push_back(geodeVertexDataPtr[i]);
/* preserve the same normals and texture coordinates */
int nNormals = shapeGeode->mNumNormals;
for (int i = 0; i < nNormals; i++)
{
mSurfNormalArray->push_back(geodeNormalDataPtr[i]);
mSurfUDirArray->push_back(geodeUDirDataPtr[i]);
mSurfVDirArray->push_back(geodeVDirDataPtr[i]);
}
int nTexcoords = shapeGeode->mNumTexcoords;
for (int i = 0; i < nTexcoords; i++) mSurfTexcoordArray->push_back(geodeTexcoordDataPtr[i]);
/* apply offset from 'gShapeCenter' to Vec3(0, 0, 0) on root level */
Matrixf transMat;
transMat.makeTranslate(-gShapeCenter);
mRootTrans->setMatrix(transMat);
/* copy CAVEGeometry objects into separate 'CAVEGeodeIconSurface' */
for (int i = 0; i < nGeoms; i++)
{
CAVEGeodeIconSurface *iconSurface = new CAVEGeodeIconSurface(&mSurfVertexArray, &mSurfNormalArray,
&mSurfTexcoordArray, &(orgGeomVector[i]), &(refGeomVector[i]));
/* 1) take record of 'iconSurface' in mCAVEGeodeIconVector; 2) add it to 'this' group */
mCAVEGeodeIconVector.push_back(iconSurface);
mRootTrans->addChild(iconSurface);
}
}
void Quat::get(Matrixf& matrix) const
{
matrix.makeRotate(*this);
}
void Quat::set(const Matrixf& matrix)
{
*this = matrix.getRotate();
}
void Render(float dt)
{
static unsigned int frameIndex = 0;
frameIndex++;
ovrFrameTiming timing = ovrHmd_BeginFrame(s_hmd, 0);
// ovrSensorState ss = ovrHmd_GetSensorState(s_hmd, timing.ScanoutMidpointSeconds);
// TODO: Use this for head tracking...
// TODO: Use player height from SDK
glBindBuffer(GL_ARRAY_BUFFER, 0);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
// render into fbo
glBindFramebuffer(GL_FRAMEBUFFER, s_fbo);
// TODO: enable this when we have more complex rendering.
glEnable(GL_DEPTH_TEST);
glDisable(GL_CULL_FACE);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
static float t = 0.0;
t += dt;
// clear render target
glViewport(0, 0, s_renderTargetSize.w, s_renderTargetSize.h);
glClearColor(s_clearColor.x, s_clearColor.y, s_clearColor.z, s_clearColor.w);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
for (int i = 0; i < 2; i++)
{
ovrEyeType eye = s_hmdDesc.EyeRenderOrder[i];
ovrPosef pose = ovrHmd_BeginEyeRender(s_hmd, eye);
glViewport(s_eyeTexture[eye].Header.RenderViewport.Pos.x,
s_eyeTexture[eye].Header.RenderViewport.Pos.y,
s_eyeTexture[eye].Header.RenderViewport.Size.w,
s_eyeTexture[eye].Header.RenderViewport.Size.h);
Quatf q(pose.Orientation.x, pose.Orientation.y, pose.Orientation.z, pose.Orientation.w);
Vector3f p(pose.Position.x, pose.Position.y, pose.Position.z);
Matrixf cameraMatrix = Matrixf::QuatTrans(q, s_cameraPos);
Matrixf viewCenter = cameraMatrix.OrthoInverse();
// let ovr compute projection matrix, cause it's hard.
ovrMatrix4f ovrProj = ovrMatrix4f_Projection(s_eyeRenderDesc[eye].Fov, 0.1f, 10000.0f, true);
// convert to abaci matrix
Matrixf projMatrix = Matrixf::Rows(Vector4f(ovrProj.M[0][0], ovrProj.M[0][1], ovrProj.M[0][2], ovrProj.M[0][3]),
Vector4f(ovrProj.M[1][0], ovrProj.M[1][1], ovrProj.M[1][2], ovrProj.M[1][3]),
Vector4f(ovrProj.M[2][0], ovrProj.M[2][1], ovrProj.M[2][2], ovrProj.M[2][3]),
Vector4f(ovrProj.M[3][0], ovrProj.M[3][1], ovrProj.M[3][2], ovrProj.M[3][3]));
// use EyeRenderDesc.ViewAdjust to do eye offset.
Matrixf viewMatrix = viewCenter * Matrixf::Trans(Vector3f(s_eyeRenderDesc[eye].ViewAdjust.x,
s_eyeRenderDesc[eye].ViewAdjust.y,
s_eyeRenderDesc[eye].ViewAdjust.z));
// compute model matrix for terminal
const float kTermScale = 0.001f;
const Vector3f termOrigin(-2 * kFeetToMeters, 6.75f * kFeetToMeters, -2.5 * kFeetToMeters);
Matrixf modelMatrix = Matrixf::ScaleQuatTrans(Vector3f(kTermScale, -kTermScale, kTermScale),
Quatf::AxisAngle(Vector3f(0, 1, 0), 0),
termOrigin);
RenderBegin();
RenderFloor(projMatrix, viewMatrix, 0.0f);
RenderTextBegin(projMatrix, viewMatrix, modelMatrix);
for (int j = 0; j < win_get_text_count(); j++)
{
gb::Text* text = (gb::Text*)win_get_text(j);
if (text)
{
RenderText(text->GetQuadVec());
}
}
RenderTextEnd();
RenderEnd();
ovrHmd_EndEyeRender(s_hmd, eye, pose, &s_eyeTexture[eye]);
}
ovrHmd_EndFrame(s_hmd);
}
/***************************************************************
* Function: preFrame()
***************************************************************/
void CaveCADBeta::preFrame()
{
/* get pointer position in world space */
Matrixf invBaseMat = PluginHelper::getWorldToObjectTransform();
Matrixf baseMat = PluginHelper::getObjectToWorldTransform();
Matrixf viewMat = PluginHelper::getHeadMat(0);
Matrixf pointerMat = TrackingManager::instance()->getHandMat(0);
Vec3 pointerPos = Vec3(0.0, 1000.0, 0.0) * pointerMat * invBaseMat;
Vec3 pointerOrg = Vec3(0.0, 0.0, 0.0) * pointerMat * invBaseMat;
/* get viewer's position in world space */
Vec3 viewOrg = viewMat.getTrans() * invBaseMat;
Vec3 viewPos = Vec3(0.0, 1000.0, 0.0) * viewMat * invBaseMat;
Vec3 viewDir = viewPos - viewOrg;
viewDir.normalize();
/* handle pointer/update events */
// coPointerButton* pointerBtn = cover->getPointerButton();
unsigned int btn = TrackingManager::instance()->getRawButtonMask();
// if (pointerBtn->wasPressed())
if (btn)
{
if (!pointerPressFlag)
{
pointerPressFlag = true;
pointerPressEvent(pointerOrg, pointerPos);
}
}
// else if (pointerBtn->wasReleased())
else if (!btn)
{
pointerReleaseEvent();
pointerPressFlag = false;
}
else
{
pointerMoveEvent(pointerOrg, pointerPos);
}
mCAVEDesigner->update(viewDir, viewPos);
/* spin wheel and top pointer buttons */
float spinX = PluginHelper::getValuator(0, 0);
float spinY = PluginHelper::getValuator(0, 1);
int pointerStat = TrackingManager::instance()->getRawButtonMask();
spinWheelEvent(spinX, spinY, pointerStat);
/* Debugging codes for model calibration
float scale = PluginHelper::getObjectScale();
Matrix xMat = PluginHelper::getObjectMatrix();
cerr << endl << "Scale = " << scale << endl;
cerr << xMat(0, 0) << " " << xMat(0, 1) << " " << xMat(0, 2) << " " << xMat(0, 3) << endl;
cerr << xMat(1, 0) << " " << xMat(1, 1) << " " << xMat(1, 2) << " " << xMat(1, 3) << endl;
cerr << xMat(2, 0) << " " << xMat(2, 1) << " " << xMat(2, 2) << " " << xMat(2, 3) << endl;
cerr << xMat(3, 0) << " " << xMat(3, 1) << " " << xMat(3, 2) << " " << xMat(3, 3) << endl;
cerr << " Frame Count = " << frameCnt++ << endl;
*/
}
/***************************************************************
* Function: applyEditorInfo()
***************************************************************/
void CAVEGeodeShape::applyEditorInfo(Vec3Array **vertexArrayPtr, Vec3Array **normalArrayPtr,
Vec3Array **udirArrayPtr, Vec3Array **vdirArrayPtr, Vec2Array **texcoordArrayPtr,
const Vec3Array *refVertexArrayPtr, const Vec3Array *refNormalArrayPtr,
const Vec3Array *refUDirArrayPtr, const Vec3Array *refVDirArrayPtr, const Vec2Array *refTexcoordArrayPtr,
const int &nVerts, EditorInfo **infoPtr, const VertexMaskingVector &vertMaskingVector)
{
/* access target and source data pointers */
Vec3 *geodeVertexDataPtr, *geodeNormalDataPtr, *geodeUDirDataPtr, *geodeVDirDataPtr;
Vec2 *geodeTexcoordDataPtr;
const Vec3 *refGeodeVertexDataPtr, *refGeodeNormalDataPtr, *refGeodeUDirDataPtr, *refGeodeVDirDataPtr;
const Vec2 *refGeodeTexcoordDataPtr;
geodeVertexDataPtr = (Vec3*) ((*vertexArrayPtr)->getDataPointer());
geodeNormalDataPtr = (Vec3*) ((*normalArrayPtr)->getDataPointer());
geodeUDirDataPtr = (Vec3*) ((*udirArrayPtr)->getDataPointer());
geodeVDirDataPtr = (Vec3*) ((*vdirArrayPtr)->getDataPointer());
geodeTexcoordDataPtr = (Vec2*) ((*texcoordArrayPtr)->getDataPointer());
refGeodeVertexDataPtr = (const Vec3*) (refVertexArrayPtr->getDataPointer());
refGeodeNormalDataPtr = (const Vec3*) (refNormalArrayPtr->getDataPointer());
refGeodeUDirDataPtr = (const Vec3*) (refUDirArrayPtr->getDataPointer());
refGeodeVDirDataPtr = (const Vec3*) (refVDirArrayPtr->getDataPointer());
refGeodeTexcoordDataPtr = (const Vec2*) (refTexcoordArrayPtr->getDataPointer());
/* implement vertex & normal updates with respect to all ActiveTypeMasking
texture coordinates are only changed in 'MOVE' and 'SCALE' operations,
texture directional vectors are only changed in 'ROTATE' operations.
*/
if ((*infoPtr)->getTypeMasking() == EditorInfo::MOVE)
{
const Vec3 offset = (*infoPtr)->getMoveOffset();
for (int i = 0; i < nVerts; i++)
{
if (vertMaskingVector[i])
{
/* apply offset values to vetex data vector */
geodeVertexDataPtr[i] = refGeodeVertexDataPtr[i] + offset;
/* apply offset values to texture coordinates, normal is not changed */
Vec3 udir = refGeodeUDirDataPtr[i];
Vec3 vdir = refGeodeVDirDataPtr[i];
Vec2 texoffset = Vec2(udir * offset, vdir * offset) / gTextureTileSize;
geodeTexcoordDataPtr[i] = refGeodeTexcoordDataPtr[i] + texoffset;
}
}
}
else if ((*infoPtr)->getTypeMasking() == EditorInfo::ROTATE)
{
const Vec3 center = (*infoPtr)->getRotateCenter();
const Vec3 axis = (*infoPtr)->getRotateAxis();
const float angle = (*infoPtr)->getRotateAngle();
Matrixf rotMat;
rotMat.makeRotate(angle, axis);
for (int i = 0; i < nVerts; i++)
{
if (vertMaskingVector[i])
{
/* update vertex list: 'translation' -> 'rotation' -> 'reversed translation' */
Vec3 pos = refGeodeVertexDataPtr[i];
geodeVertexDataPtr[i] = (pos - center) * rotMat + center;
/* update normal and u, v-direction vectors with single rotations */
Vec3 norm = refGeodeNormalDataPtr[i];
Vec3 udir = refGeodeUDirDataPtr[i];
Vec3 vdir = refGeodeVDirDataPtr[i];
geodeNormalDataPtr[i] = norm * rotMat;
geodeUDirDataPtr[i] = udir * rotMat;
geodeVDirDataPtr[i] = vdir * rotMat;
}
}
}
else if ((*infoPtr)->getTypeMasking() == EditorInfo::SCALE)
{
const Vec3 center = (*infoPtr)->getScaleCenter();
const Vec3 scale = (*infoPtr)->getScaleVect();
Matrixf scaleMat;
scaleMat.makeScale(scale);
for (int i = 0; i < nVerts; i++)
{
if (vertMaskingVector[i])
{
/* update vertex list: 'translation' -> 'scaling' -> 'reversed translation' */
Vec3 pos = refGeodeVertexDataPtr[i];
geodeVertexDataPtr[i] = (pos - center) * scaleMat + center;
Vec3 offset = geodeVertexDataPtr[i] - pos;
/* update texture coordinates 'u', 'v', normal and u, v-direction vectors are not changed */
Vec3 udir = refGeodeUDirDataPtr[i];
Vec3 vdir = refGeodeVDirDataPtr[i];
Vec2 texoffset = Vec2(udir * offset, vdir * offset) / gTextureTileSize;
geodeTexcoordDataPtr[i] = refGeodeTexcoordDataPtr[i] + texoffset;
}
}
}
else return;
}
/**
* Main application entry point
* @param argumentCount The amount of arguments comment in
* @param arguments The list of incomming arguments
* @returns SUCCESS or FAILURE
*/
int main(int argumentCount, char **arguments)
{
try
{
if (argumentCount != 2) throw runtime_error("USAGE: q5 <fileName>");
Matrixf vertices = ReadFile(arguments[1]);
// ---------------- Compute the robust normal ----------------
// compute centroid
Matrixf centroid(vertices.ncols(), 1);
for (unsigned int c = 0; c < vertices.ncols(); c++)
{
float total = 0;
for (unsigned int r = 0; r < vertices.nrows(); r++)
total += vertices(r, c);
centroid(c, 0) = total / vertices.nrows();
}
// find normal of our plane (formed by points 0, 1, and C)
Matrixf normEst = cross(subtract(GetMatrixRow(vertices, 0), centroid), subtract(GetMatrixRow(vertices, 1), centroid));
// find the absolute largest component (x,y,z) of the normal
for (unsigned int i = 0; i < 3; i++)
normEst(i, 0) = abs(normEst(i, 0));
int componentI;
if (normEst(0,0) > normEst(1,0) && normEst(0,0) > normEst(2,0))
componentI = 0;
else if (normEst(1,0) > normEst(0,0) && normEst(1,0) > normEst(2,0))
componentI = 1;
else
componentI = 2;
// copy vertices -- removing the largest component
unsigned int tempCount;
Matrixf projVertices(vertices.nrows(), vertices.ncols() - 1);
for (unsigned int r = 0; r < projVertices.nrows(); r++)
{
tempCount = 0;
for (unsigned int c = 0; c < vertices.ncols(); c++)
{
if (c != componentI)
{
projVertices(r, tempCount) = vertices(r, c);
tempCount++;
}
}
}
Matrixf projCentroid(centroid.nrows() - 1, 1);
tempCount = 0;
for (unsigned int r = 0; r < centroid.nrows(); r++)
{
if (r != componentI)
{
projCentroid(tempCount, 0) = centroid(r, 0);
tempCount++;
}
}
// calculate the counter-clockwise ordering of the vertices
Matrixf vecOA = subtract(GetMatrixRow(projVertices, 0), projCentroid);
Matrixf rotate(2, 2);
rotate(0, 0) = 0; rotate(0, 1) = -1;
rotate(1, 0) = 1; rotate(1, 1) = 0;
IndexAngle indexAng[5];
indexAng[0].index = 0;
indexAng[0].angle = 0.0f;
for (unsigned int r = 1; r < projVertices.nrows(); r++) // for each other vertex
{
Matrixf temp = subtract(GetMatrixRow(projVertices, r), projCentroid);
float angle = acos(dot(temp, vecOA) / (length(temp) * length(vecOA)));
// rotate ccw pi/2
Matrixf tempRot = multiply(rotate, temp);
float angleRot = acos(dot(tempRot, vecOA) / (length(tempRot) * length(vecOA)));
if ((angle < M_PI_2 && angleRot < M_PI_2) || (angle > M_PI_2 && angleRot < M_PI_2)) // angle on right
angle = (M_PI * 2) - angle;
indexAng[r].index = r;
indexAng[r].angle = angle;
}
// sort by angle size
sort(indexAng, indexAng + 5, &angle_sorter);
// compute the normal at each vertex
Matrixf normalAtVert(vertices.nrows(), vertices.ncols());
Matrixf normRunning(3, 1);
for (unsigned int i = 0; i < vertices.nrows(); i++)
{
Matrixf vecToPrev(3, 1);
if (i == 0) // wrap arround
vecToPrev = subtract(GetMatrixRow(vertices, indexAng[4].index), GetMatrixRow(vertices, indexAng[i].index));
else
vecToPrev = subtract(GetMatrixRow(vertices, indexAng[i-1].index), GetMatrixRow(vertices, indexAng[i].index));
Matrixf vecToNext(3, 1);
if (i == 4) // wrap around
vecToNext = subtract(GetMatrixRow(vertices, indexAng[0].index), GetMatrixRow(vertices, indexAng[i].index));
else
vecToNext = subtract(GetMatrixRow(vertices, indexAng[i+1].index), GetMatrixRow(vertices, indexAng[i].index));
Matrixf thisNorm = cross(vecToNext, vecToPrev);
normRunning = add(normRunning, thisNorm); // for computing the robust normal
thisNorm = normalize(thisNorm);
for (unsigned int c = 0; c < 3; c++)
normalAtVert(i, c) = thisNorm(c, 0); // for finding the weakest vertex
}
// normalise the total
Matrixf normRobust = normalize(normRunning);
cout << "Robust Normal: " << transpose(normRobust);
// ---------------- compute the new robust normal ----------------
// find weakest vertex
IndexAngle normStrength[5];
for (unsigned int i = 0; i < vertices.nrows(); i++)
{
normStrength[i].index = indexAng[i].index;
normStrength[i].angle = acos(dot(normalize(GetMatrixRow(normalAtVert, i)), normRobust));
}
// sort by angle size (largest to smallest)
sort(normStrength, normStrength + 5, &angle_sorter_inv);
unsigned int weakestIndex = normStrength[0].index;
cout << "Weakest Vertex: " << transpose(GetMatrixRow(vertices, weakestIndex));
// remove the weakest vertex
IndexAngle indexAng2[4];
tempCount = 0;
for (unsigned int i = 0; i < 5; i++)
{
if (indexAng[i].index == weakestIndex)
continue;
indexAng2[tempCount] = indexAng[i];
tempCount++;
}
// compute the normal at each vertex
Matrixf norm2Running(3, 1);
for (unsigned int i = 0; i < vertices.nrows() - 1; i++)
{
Matrixf vecToPrev(3, 1);
if (i == 0) // wrap arround
vecToPrev = subtract(GetMatrixRow(vertices, indexAng2[3].index), GetMatrixRow(vertices, indexAng2[i].index));
else
vecToPrev = subtract(GetMatrixRow(vertices, indexAng2[i-1].index), GetMatrixRow(vertices, indexAng2[i].index));
Matrixf vecToNext(3, 1);
if (i == 3) // wrap around
vecToNext = subtract(GetMatrixRow(vertices, indexAng2[0].index), GetMatrixRow(vertices, indexAng2[i].index));
else
vecToNext = subtract(GetMatrixRow(vertices, indexAng2[i+1].index), GetMatrixRow(vertices, indexAng2[i].index));
norm2Running = add(norm2Running, cross(vecToNext, vecToPrev));
}
// normalise the total
Matrixf norm2Robust = normalize(norm2Running);
cout << "New Robust Normal: " << transpose(norm2Robust);
}
catch(runtime_error error)
{
cerr << "ERROR: " << error.what() << endl;
return FAILURE;
}
return SUCCESS;
}
