// gaussian elimination - main function
int WINAPI GaussianElimination(double ** ppMatrix, int iRows, int iColumns, GAUSSIAN_ELIMINATION_OUTPUT * pOutputBuffer)
{
if(ppMatrix == NULL) {
return -1;
}
if(pOutputBuffer->ppSMatrix != NULL) {
iColumns += iRows;
}
SetMatrixDimensions(iRows, iColumns);
double ** ppWorkMatrix;
if(pOutputBuffer->ppSMatrix == NULL) {
ppWorkMatrix = pOutputBuffer->ppReducedEcholonForm;
} else {
ppWorkMatrix = AllocateMatrixMemory(iColumns + iRows, iRows);
}
if(!ppWorkMatrix) {
return -1;
}
if(pOutputBuffer->ppSMatrix != NULL) {
for(int i = (iColumns - iRows); i < iColumns; i++) {
for(int g = 0; g < iRows; g++) {
if(g == i - iColumns + iRows) {
ppWorkMatrix[i][g] = 1.0f;
} else {
ppWorkMatrix[i][g] = 0.0f;
}
}
}
CopyMatrix(ppWorkMatrix, ppMatrix, iRows, iColumns - iRows);
} else {
CopyMatrix(ppWorkMatrix, ppMatrix, iRows, iColumns);
}
POSITION posPivot = StepOne(ppWorkMatrix);
//printf("%lf, %lf", ppWorkMatrix[0][0], ppWorkMatrix[0][1]);
StepTwo(ppWorkMatrix, &posPivot);
//printf("%lf, %lf", ppWorkMatrix[0][0], ppWorkMatrix[0][1]);
if(StepThree(ppWorkMatrix, posPivot) != 0) {
return -1;
}
//printf("%lf, %lf", ppWorkMatrix[0][0], ppWorkMatrix[0][1]);
StepFour(ppWorkMatrix, posPivot);
//printf("%lf, %lf", ppWorkMatrix[0][0], ppWorkMatrix[0][1]);
POSITION_ARRAY pivots = StepFive(ppWorkMatrix, posPivot);
//printf("%lf, %lf", ppWorkMatrix[0][0], ppWorkMatrix[0][1]);
AddPosition(&pivots, posPivot);
StepSix(ppWorkMatrix, pivots);
//printf("%lf, %lf", ppWorkMatrix[0][0], ppWorkMatrix[0][1]);
if(pOutputBuffer->ppSMatrix) {
DivideOutputAndSMatrix(ppWorkMatrix, pOutputBuffer);
} else {
CopyMatrix(pOutputBuffer->ppReducedEcholonForm, ppWorkMatrix, iRows, iColumns);
}
return 0;
}
// Kalman filter initilizations
void KalmanInit(struct KalmanFilter *kal, int variables, int measurements,
struct Matrix updateMatrix,
struct Matrix extractionMatrix,
struct Matrix covarianceMatrixX,
struct Matrix covarianceMatrixZ,
struct Matrix moveVector)
{
CreateZerosMatrix(&kal->meanVector, variables, 1);
CreateZerosMatrix(&kal->measurementVector, measurements, 1);
CopyMatrix(&kal->updateMatrix, updateMatrix);
CopyMatrix(&kal->extractionMatrix, extractionMatrix);
CopyMatrix(&kal->moveVector, moveVector);
CopyMatrix(&kal->covarianceMatrixX, covarianceMatrixX);
CopyMatrix(&kal->covarianceMatrixZ, covarianceMatrixZ);
}
void display(void)
{
// clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
GLfloat modelView[16], camMatrix[16], cam_Matrix_skybox[16], total[16];
printError("pre display");
check_keys();
t += 1;
glUseProgram(program);
// Build matrix
lookAt(&cam_pos, &obj_pos, up.x, up.y, up.z, camMatrix);
CopyMatrix(camMatrix, cam_Matrix_skybox);
cam_Matrix_skybox[3] = 0;
cam_Matrix_skybox[7] = 0;
cam_Matrix_skybox[11] = 0;
cam_Matrix_skybox[15] = 1;
// disable z-buffer for skybox
glDisable(GL_DEPTH_TEST);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Draw skybox
// Set the skybox variable
glUniform1i(glGetUniformLocation(program, "skybox"), 1);
T(0,-0.5,0,modelView);
Mult(cam_Matrix_skybox, modelView, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
glBindTexture(GL_TEXTURE_2D, skytex);
DrawModel(skybox, program, "inPosition", "inNormal", "inTexCoord");
glUniform1i(glGetUniformLocation(program, "skybox"), 0);
glEnable(GL_DEPTH_TEST);
// Bind terrain
IdentityMatrix(modelView);
Mult(camMatrix, modelView, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
// Bind Our Texture tex1
glBindTexture(GL_TEXTURE_2D, tex1);
DrawModel(tm, program, "inPosition", "inNormal", "inTexCoord");
// Draw sphere
sphere_pos = update_sphere(t, total);
sphere_pos.y = calculate_height(sphere_pos.x, sphere_pos.z, ttex.width, vertexArray);
T(sphere_pos.x, sphere_pos.y, sphere_pos.z, trans);
Mult(camMatrix, trans, total);
//Ry(0.01*t, roty);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
glBindTexture(GL_TEXTURE_2D, spheretex);
DrawModel(sphere, program, "inPosition", "inNormal", "inTexCoord");
printError("display 2");
glutSwapBuffers();
}
void ODERigidObject::Create(dWorldID worldID,dSpaceID space,bool useBoundaryLayer)
{
Clear();
spaceID = space;
bodyID = dBodyCreate(worldID);
dMass mass;
mass.mass = obj.mass;
//NOTE: in ODE, COM must be zero vector! -- we take care of this by shifting geometry
//CopyVector3(mass.c,obj.com);
mass.c[0] = mass.c[1] = mass.c[2] = 0; mass.c[3] = 1.0;
CopyMatrix(mass.I,obj.inertia);
int res=dMassCheck(&mass);
if(res != 1) {
fprintf(stderr,"Uh... mass is not considered to be valid by ODE?\n");
std::cerr<<"Inertia: "<<obj.inertia<<std::endl;
}
dBodySetMass(bodyID,&mass);
geometry = new ODEGeometry;
geometry->Create(&obj.geometry,spaceID,-obj.com,useBoundaryLayer);
dGeomSetBody(geometry->geom(),bodyID);
dGeomSetData(geometry->geom(),(void*)-1);
geometry->SetPadding(defaultPadding);
geometry->surf().kRestitution = obj.kRestitution;
geometry->surf().kFriction = obj.kFriction;
geometry->surf().kStiffness = obj.kStiffness;
geometry->surf().kDamping = obj.kDamping;
SetTransform(obj.T);
}
/* SetCovs: set covariance values in hmm */
void SetCovs(void)
{
int i,s,m;
StateElem *se;
StreamElem *ste;
MixtureElem *me;
MixPDF *mp;
CalcCovs();
if (trace&T_TOP) {
printf("Updating HMM ");
if (meanUpdate) printf("Means and ");
printf("Covariances\n");
}
for (i=2,se=hmmLink->svec+2; i < hmmLink->numStates; i++,se++)
for (s=1,ste=se->info->pdf+1; s <= hset.swidth[0]; s++,ste++)
for (m=1,me = ste->spdf.cpdf+1; m<=ste->nMix; m++, me++) {
mp = me->mpdf;
if (meanUpdate && !IsSeenV(mp->mean)){ /* meanSum now holds mean */
CopyVector(accs[s].meanSum,mp->mean);
TouchV(mp->mean);
}
if (!IsSeenV(mp->cov.var)){
if (mp->ckind==FULLC)
CopyMatrix(accs[s].fixed.inv,mp->cov.inv);
else if (fullcNeeded[s]) /* dont need full cov, but its all we have */
TriDiag2Vector(accs[s].fixed.inv,mp->cov.var);
else
CopyVector(accs[s].fixed.var,mp->cov.var);
TouchV(mp->cov.var);
}
}
ClearSeenFlags(&hset,CLR_ALL);
}
void ODERigidObject::SetTransform(const RigidTransform& T)
{
Vector3 comPos = T*obj.com;
dBodySetPosition(bodyID,comPos.x,comPos.y,comPos.z);
dMatrix3 rot;
CopyMatrix(rot,T.R);
dBodySetRotation(bodyID,rot);
}
void ODERigidObject::GetTransform(RigidTransform& T) const
{
const dReal* pos = dBodyGetPosition(bodyID);
Vector3 comPos(pos[0],pos[1],pos[2]);
const dReal* rot = dBodyGetRotation(bodyID);
CopyMatrix(T.R,rot);
T.t = comPos - T.R*obj.com;
}
SpringHook::SpringHook(dBodyID _body,const Vector3& _worldpt,const Vector3& _target,Real _k)
:body(_body),target(_target),k(_k)
{
Matrix3 R;
Vector3 t;
CopyVector(t,dBodyGetPosition(body));
CopyMatrix(R,dBodyGetRotation(body));
R.mulTranspose(_worldpt-t,localpt);
}
// Kalman Update Step
// We "update" given what we get for data
// The filter will use the data given to lower our
// uncertainty (aka. covariance)
void KalmanUpdate(struct KalmanFilter *kal, struct Matrix meas)
{
struct Matrix y, S, extTran, K, Sinv, x_next, P_next;
CreateBlankMatrix(y);
CreateBlankMatrix(S);
CreateBlankMatrix(extTran);
CreateBlankMatrix(K);
CreateBlankMatrix(Sinv);
CreateBlankMatrix(x_next);
CreateBlankMatrix(P_next);
CopyMatrix(&kal->measurementVector, meas);
// Find the difference between the move (measurement)
// and what we predicted (extraction * data)
MatrixMult(&y, kal->extractionMatrix, kal->meanVector);
MatrixSub(&y, kal->measurementVector, y);
// The Covariance of the move
MatrixMult(&S, kal->extractionMatrix, kal->covarianceMatrixX);
MatrixTranspose(&extTran, kal->extractionMatrix);
MatrixMult(&S, S, extTran);
MatrixAdd(&S, S, kal->covarianceMatrixZ);
// Kalman Gain
MatrixInv(&Sinv, S);
MatrixMult(&K, kal->covarianceMatrixX, extTran);
MatrixMult(&K, K, Sinv);
// Figure out mean and covariance results
MatrixMult(&x_next, K, y);
MatrixAdd(&x_next, kal->meanVector, x_next);
MatrixMult(&P_next, kal->covarianceMatrixX, extTran);
MatrixMult(&P_next, P_next, Sinv);
MatrixMult(&P_next, P_next, kal->extractionMatrix);
MatrixMult(&P_next, P_next, kal->covarianceMatrixX);
MatrixSub(&P_next, kal->covarianceMatrixX, P_next);
// Copy results to the kalmanfilter class
CopyMatrixByValue(&kal->meanVector, x_next);
CopyMatrixByValue(&kal->covarianceMatrixX, P_next);
// Delete matricies so we don't have memory leaks..
DeleteMatrix(&y);
DeleteMatrix(&S);
DeleteMatrix(&extTran);
DeleteMatrix(&K);
DeleteMatrix(&Sinv);
DeleteMatrix(&x_next);
DeleteMatrix(&P_next);
}
void DexMouseTracker::GetUnitTransform( int unit, Vector3 &offset, Matrix3x3 &rotation ) {
// Simulate a placement error by swapping the units;
if ( !IsDlgButtonChecked( dlg, IDC_CODA_POSITIONED ) ) {
unit = 1 - unit;
}
// Return constant values for the Coda unit placement.
// The real tracker will compute these from the Coda transformations.
if ( TRACKER_ALIGNED_SUPINE == SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) {
CopyVector( offset, SimulatedSupineCodaOffset[unit] );
CopyMatrix( rotation, SimulatedSupineCodaRotation[unit] );
}
else {
CopyVector( offset, SimulatedUprightCodaOffset[unit] );
CopyMatrix( rotation, SimulatedUprightCodaRotation[unit] );
}
}
OpenGLObject::OpenGLObject( void ) {
texture = NULL;
umag = 1.0;
vmag = 1.0;
enabled = true;
list = -1;
CopyVector( position, zeroVector );
CopyMatrix( orientation, identityMatrix );
CopyVector( offset, zeroVector );
CopyMatrix( attitude, identityMatrix );
init_gl_rotation( gl_attitude );
init_gl_rotation( gl_orientation );
init_gl_displacement( gl_position );
init_gl_displacement( gl_offset );
SetColor( 0.0, 0.0, 0.0, USE_PARENT_COLOR );
}
void SpringHook::Step(Real dt)
{
Matrix3 R;
Vector3 t;
Vector3 wp,f;
CopyVector(t,dBodyGetPosition(body));
CopyMatrix(R,dBodyGetRotation(body));
wp = R*localpt+t;
f = k*(target-wp);
//cout<<"Target "<<target<<", world point "<<wp<<", force "<<f<<endl;
dBodyAddForceAtPos(body,f.x,f.y,f.z,wp.x,wp.y,wp.z);
}
/* CloneSMatrix: return a clone of given Matrix */
SMatrix CloneSMatrix(MemHeap *hmem, SMatrix s, Boolean sharing)
{
SMatrix t; /* the target */
if (s==NULL) return NULL;
if (GetUse(s)>0 && sharing) {
IncUse(s);
return s;
}
t = CreateSMatrix(hmem,NumRows(s),NumCols(s));
CopyMatrix(s,t);
return t;
}
void OpenGLObject::SetAttitude( Matrix3x3 m ) {
int i, j;
CopyMatrix( attitude, m );
for ( i = 0; i < 3; i++ ) {
for ( j = 0; j < 3; j++ ) {
gl_attitude[ i * 4 + j ] = attitude[i][j];
}
gl_attitude[ i * 4 + j ] = 0.0;
}
for ( j = 0; j < 3; j++ ) {
gl_attitude[ i * 4 + j ] = 0.0;
}
gl_attitude[ i * 4 + j ] = 1.0;
}
void OpenGLObject::SetOrientation( Matrix3x3 m ) {
int i, j;
CopyMatrix( orientation, m );
for ( i = 0; i < 3; i++ ) {
for ( j = 0; j < 3; j++ ) {
gl_orientation[ i * 4 + j ] = orientation[i][j];
}
gl_orientation[ i * 4 + j ] = 0.0;
}
for ( int j = 0; j < 3; j++ ) {
gl_orientation[ i * 4 + j ] = 0.0;
}
gl_orientation[ i * 4 + j ] = 1.0;
}
// Viewpoints set the inverse as the rotation matrix.
void Viewpoint::SetAttitude( Matrix3x3 m ) {
int i, j;
Matrix3x3 inverse;
CopyMatrix( attitude, m );
InvertMatrix( inverse, m );
for ( i = 0; i < 3; i++ ) {
for ( j = 0; j < 3; j++ ) {
gl_attitude[ i * 4 + j ] = inverse[i][j];
}
gl_attitude[ i * 4 + j ] = 0.0;
}
for ( int j = 0; j < 3; j++ ) {
gl_attitude[ i * 4 + j ] = 0.0;
}
gl_attitude[ i * 4 + j ] = 1.0;
}
// Viewpoints set the inverse as the rotation matrix.
void Viewpoint::SetOrientation( Matrix3x3 m ) {
int i, j;
Matrix3x3 inverse;
CopyMatrix( orientation, m );
InvertMatrix( inverse, m );
for ( i = 0; i < 3; i++ ) {
for ( j = 0; j < 3; j++ ) {
gl_orientation[ i * 4 + j ] = inverse[i][j];
}
gl_orientation[ i * 4 + j ] = 0.0;
}
for ( int j = 0; j < 3; j++ ) {
gl_orientation[ i * 4 + j ] = 0.0;
}
gl_orientation[ i * 4 + j ] = 1.0;
}
void tdiff( double dtrad, double radtime, double depositionFraction )
{
char *me = "tdiff";
char *bufname = "RBNDCOM";
int comerr = 0;
int iblk ;
int my_nblk = nblk ;
double fraction ;
double myflops = 0.0;
FT_INITIALIZE(me, gv_hash_tbl) ;
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
ChemPotCalc( domains[iblk].den, domains[iblk].tmat, &domains[iblk] ) ;
}
RadiationAllocate( &rblk, &rblkbak, &cblk, &tblk, (ifedif + ifidif) ) ;
comerr += rbndcom(RBNDCOM2,COM_RECV,0);
comerr += rbndcom(RBNDCOM2,COM_SEND,0);
comerr += rbndcom(RBNDCOM2,COM_WAIT_RECV,0);
comerr += rbndcom(RBNDCOM2,COM_WAIT_SEND,0);
if ( ndims == 2 ) {
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
volcal2d( domains[iblk].vol, domains[iblk].area,
domains[iblk].x , domains[iblk].y,
&domains[iblk] ) ;
}
}
if ( ndims == 3 ) {
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
volcal3d( domains[iblk].vol, domains[iblk].x, domains[iblk].y,
domains[iblk].z, &domains[iblk] ) ;
}
}
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
int length = domains[iblk].namix ;
DivgradDriver( domains[iblk].x, domains[iblk].y, domains[iblk].z,
&domains[iblk], &rblk[iblk] ) ;
if ( (ifedif + ifidif) > 0 ) {
CopyMatrix( &rblkbak[iblk], &rblk[iblk], length, ndims ) ;
}
}
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
refzq( domains[iblk].vol, &domains[iblk] ) ;
}
if ( (ifidif > 0) && (iftion == 1) ) {
tblkinit( tblk ) ;
IonConduction( rblk, cblk, tblk, domains, dtrad, radtime ) ;
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
CopyMatrix( &rblk[iblk], &rblkbak[iblk],
domains[iblk].namix, ndims ) ;
}
}
if ( ifedif > 0 ) {
tblkinit( tblk ) ;
ElectronConduction( rblk, cblk, tblk, domains, dtrad, radtime ) ;
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
CopyMatrix( &rblk[iblk], &rblkbak[iblk],
domains[iblk].namix, ndims ) ;
}
}
if ( ngroup == 1 ) {
tblkinit( tblk ) ;
rdiff( rblk, cblk, tblk, domains, dtrad, radtime, depositionFraction ) ;
}
RadiationFree( rblk, rblkbak, cblk, tblk, (ifedif + ifidif) ) ;
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
setpz1( domains[iblk].vol, 0.0, &domains[iblk] ) ;
setpz2( domains[iblk].vol, 0.0, &domains[iblk] ) ;
}
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
setpz1( domains[iblk].den, 0.0, &domains[iblk] ) ;
setpz2( domains[iblk].den, 0.0, &domains[iblk] ) ;
}
#include "pardo.h"
for ( iblk = 0 ; iblk < my_nblk ; iblk++ ) {
int i;
int n;
int *ndx;
n = domains[iblk].rlen[0] ;
ndx = domains[iblk].rndx[0] ;
for ( i = 0 ; i < n ; i++ ) {
domains[iblk].ireg[ndx[i]] = 0 ;
}
}
FT_FINALIZE(me, gv_hash_tbl, myflops) ;
}
void OpenGLObject::GetOrientation( Matrix3x3 m ) {
CopyMatrix( m, orientation );
}
void display(void) {
int i, k;
/* clear the screen*/
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glUseProgram(program);
GLfloat t = (GLfloat)glutGet(GLUT_ELAPSED_TIME);
camPos += camMod;
xValue += xModify * speed;
yValue += yModify;
zValue += zModify * speed;
if (yModify == 0) {
yCamPos = yValue+2;
}
SetVector(xValue + 5 * cos(camPos), yCamPos, zValue + 5 * sin(camPos), &p);
SetVector(xValue, yCamPos + 0.5, zValue, &l);
lookAt(&p, &l, 0.0, 1.0, 0.0, cam);
GLfloat tmp[16];
CopyMatrix(cam, tmp);
tmp[3] = 0;
tmp[7] = 0;
tmp[11] = 0;
glUniformMatrix4fv(glGetUniformLocation(program, "camMatrix"), 1, GL_TRUE, tmp);
/* ================================================================== */
glDisable(GL_DEPTH_TEST);
glDisable(GL_CULL_FACE);
T(0, 0, 0, trans);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, skyBoxTex);
glUniform1i(glGetUniformLocation(program, "texUnit"), 0); // Texture unit 0
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, trans);
DrawModel(skyBox, program, "inPosition", "inNormal", "inTexCoord");
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
/* ================================================================== */
glUniformMatrix4fv(glGetUniformLocation(program, "camMatrix"), 1, GL_TRUE, cam);
T(0, 0, 0, trans);
S(150,0, 150, shear);
Mult(trans, shear, total);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, groundTex);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
DrawModel(ground, program, "inPosition", "inNormal", "inTexCoord");
/* ================================================================== */
T(10.0f, 5.0f, 0.0f, trans);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, groundTex);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, trans);
DrawModel(ground, program, "inPosition", "inNormal", "inTexCoord");
T(0.0f, 10.0f, 10.0f, trans);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, groundTex);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, trans);
DrawModel(ground, program, "inPosition", "inNormal", "inTexCoord");
T(-10.0f, 0.0f, 0.0f, trans);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, groundTex);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, trans);
DrawModel(ground, program, "inPosition", "inNormal", "inTexCoord");
T(0.0f, 0.0f, -15.0f, trans);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, groundTex);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, trans);
DrawModel(ground, program, "inPosition", "inNormal", "inTexCoord");
/* ================================================================== */
glUseProgram(programShade);
glUniform3fv(glGetUniformLocation(programShade, "inCam"), 3, &p);
glUniformMatrix4fv(glGetUniformLocation(programShade, "camMatrix"), 1, GL_TRUE, cam);
T(-3.9, 0, 0, trans);
S(0.8, 0.8, 0.8, shear);
Mult(trans, shear, total);
//Ry(M_PI, rot);
//Mult(total, rot, total);
glUniformMatrix4fv(glGetUniformLocation(programShade, "mdlMatrix"), 1, GL_TRUE, total);
DrawModel(windmillRoof, programShade, "inPosition", "inNormal", "inTexCoord");
T(-3.9, 0, 0, trans);
S(0.8, 0.8, 0.8, shear);
Mult(trans, shear, total);
//Ry(M_PI, rot);
//Mult(total, rot, total);
glUniformMatrix4fv(glGetUniformLocation(programShade, "mdlMatrix"), 1, GL_TRUE, total);
DrawModel(windmillBalcony, programShade, "inPosition", "inNormal", "inTexCoord");
T(-3.9, 0, 0, trans);
S(0.8, 0.8, 0.8, shear);
Mult(trans, shear, total);
//Ry(M_PI, rot);
//Mult(total, rot, total);
glUniformMatrix4fv(glGetUniformLocation(programShade, "mdlMatrix"), 1, GL_TRUE, total);
DrawModel(windmillWalls, programShade, "inPosition", "inNormal", "inTexCoord");
for (i = 0; i < 4; i++) {
T(0, 7.4, 0, trans);
S(0.5, 0.5, 0.5, shear);
Mult(trans, shear, total);
Rx(i * PI / 2 + t/1000, rot);
Mult(total, rot, total);
glUniformMatrix4fv(glGetUniformLocation(programShade, "mdlMatrix"), 1, GL_TRUE, total);
DrawModel(blade, programShade, "inPosition", "inNormal", "inTexCoord");
}
/* ================================================================== */
glUseProgram(programMultitex);
glEnable(GL_BLEND);
//glDisable(GL_CULL_FACE);
//glDisable(GL_DEPTH_TEST);
glUniformMatrix4fv(glGetUniformLocation(programMultitex, "camMatrix"), 1, GL_TRUE, cam);
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D, bunnyTex);
glActiveTexture(GL_TEXTURE2);
glBindTexture(GL_TEXTURE_2D, skyBoxTex);
glUniform1i(glGetUniformLocation(programMultitex, "texUnit"), 1); // Texture unit 1
glUniform1i(glGetUniformLocation(programMultitex, "texUnit2"), 2); // Texture unit 2
yValue += yModify;
if (gravity < 0 && yValue > 0.5) {
gravity += 0.03;
yModify -= gravity;
} else if (yValue > 0.5) {
gravity += 0.006;
yModify -= gravity;
} else {
gravity = -0.28;
yValue = 0.55;
yModify = 0.0;
}
// for (k = -7; k < 7; k++) {
for (i = 2; i > -2; i--) {
// T(i * 10, yValue, k * 10, trans);
T(i * 10, 2.5, 10, trans);
S(5,5,5, shear);
Mult(trans, shear, total);
glUniformMatrix4fv(glGetUniformLocation(programMultitex, "mdlMatrix"), 1, GL_TRUE, total);
DrawModel(bunny, programMultitex, "inPosition", "inNormal", "inTexCoord");
}
// }
//glEnable(GL_DEPTH_TEST);
//glEnable(GL_CULL_FACE);
glDisable(GL_BLEND);
glFlush();
// glutSwapBuffers();
}
void OpenGLObject::GetAttitude( Matrix3x3 m ) {
CopyMatrix( m, attitude );
}
void MatrixMultiply(struct matrixch *m1, struct matrixch *m2, struct matrixch *m3)
{
MATRIXCH TmpMat;
/* m11'' = c1.r1' */
TmpMat.mat11=MUL_FIXED(m1->mat11,m2->mat11);
TmpMat.mat11+=MUL_FIXED(m1->mat21,m2->mat12);
TmpMat.mat11+=MUL_FIXED(m1->mat31,m2->mat13);
/* m12'' = c2.r1' */
TmpMat.mat12=MUL_FIXED(m1->mat12,m2->mat11);
TmpMat.mat12+=MUL_FIXED(m1->mat22,m2->mat12);
TmpMat.mat12+=MUL_FIXED(m1->mat32,m2->mat13);
/* m13'' = c3.r1' */
TmpMat.mat13=MUL_FIXED(m1->mat13,m2->mat11);
TmpMat.mat13+=MUL_FIXED(m1->mat23,m2->mat12);
TmpMat.mat13+=MUL_FIXED(m1->mat33,m2->mat13);
/* m21'' = c1.r2' */
TmpMat.mat21=MUL_FIXED(m1->mat11,m2->mat21);
TmpMat.mat21+=MUL_FIXED(m1->mat21,m2->mat22);
TmpMat.mat21+=MUL_FIXED(m1->mat31,m2->mat23);
/* m22'' = c2.r2' */
TmpMat.mat22=MUL_FIXED(m1->mat12,m2->mat21);
TmpMat.mat22+=MUL_FIXED(m1->mat22,m2->mat22);
TmpMat.mat22+=MUL_FIXED(m1->mat32,m2->mat23);
/* m23'' = c3.r2' */
TmpMat.mat23=MUL_FIXED(m1->mat13,m2->mat21);
TmpMat.mat23+=MUL_FIXED(m1->mat23,m2->mat22);
TmpMat.mat23+=MUL_FIXED(m1->mat33,m2->mat23);
/* m31'' = c1.r3' */
TmpMat.mat31=MUL_FIXED(m1->mat11,m2->mat31);
TmpMat.mat31+=MUL_FIXED(m1->mat21,m2->mat32);
TmpMat.mat31+=MUL_FIXED(m1->mat31,m2->mat33);
/* m32'' = c2.r3' */
TmpMat.mat32=MUL_FIXED(m1->mat12,m2->mat31);
TmpMat.mat32+=MUL_FIXED(m1->mat22,m2->mat32);
TmpMat.mat32+=MUL_FIXED(m1->mat32,m2->mat33);
/* m33'' = c3.r3' */
TmpMat.mat33=MUL_FIXED(m1->mat13,m2->mat31);
TmpMat.mat33+=MUL_FIXED(m1->mat23,m2->mat32);
TmpMat.mat33+=MUL_FIXED(m1->mat33,m2->mat33);
/* Finally, copy TmpMat to m3 */
CopyMatrix(&TmpMat, m3);
}
int DexApparatus::CheckMovementAmplitude( double min, double max,
double dirX, double dirY, double dirZ,
const char *msg, const char *picture ) {
const char *fmt;
bool error = false;
int i, k, m;
int first, last;
double N = 0.0, sd;
Matrix3x3 Sxy;
Vector3 delta, mean;
Vector3 direction, vect;
// TODO: Should normalize the direction vector here.
direction[X] = dirX;
direction[Y] = dirY;
direction[Z] = dirZ;
// First we should look for the start and end of the actual movement based on
// events such as when the subject reaches the first target.
FindAnalysisFrameRange( first, last );
// Compute the mean position.
N = 0.0;
CopyVector( mean, zeroVector );
for ( i = first; i < last; i++ ) {
if ( acquiredManipulandumState[i].visibility ) {
if ( abs( acquiredManipulandumState[i].position[X] ) > 1000 ) {
i = i;
}
N++;
AddVectors( mean, mean, acquiredManipulandumState[i].position );
}
}
// If there is no valid position data, signal an error.
if ( N <= 0.0 ) {
monitor->SendEvent( "Movement extent - No valid data." );
sd = 0.0;
error = true;
}
else {
// This is the mean.
ScaleVector( mean, mean, 1.0 / N );;
// Compute the sums required for the variance calculation.
CopyMatrix( Sxy, zeroMatrix );
N = 0.0;
for ( i = first; i < last; i ++ ) {
if ( acquiredManipulandumState[i].visibility ) {
N++;
SubtractVectors( delta, acquiredManipulandumState[i].position, mean );
for ( k = 0; k < 3; k++ ) {
for ( m = 0; m < 3; m++ ) {
Sxy[k][m] += delta[k] * delta[m];
}
}
}
}
// If we have some data, compute the directional variance and then
// the standard deviation along that direction;
// This is just a scalar times a matrix.
// Sxy has to be a double or we risk underflow when computing the sums.
for ( k = 0; k < 3; k++ ) {
vect[k] = 0;
for ( m = 0; m < 3; m++ ) {
Sxy[k][m] /= N;
}
}
// This is a matrix multiply.
for ( k = 0; k < 3; k++ ) {
for ( m = 0; m < 3; m++ ) {
vect[k] += Sxy[m][k] * direction[m];
}
}
// Compute the length of the vector, which is the variance along
// the specified direction. Then take the square root of that
// magnitude to get the standard deviation along that direction.
sd = sqrt( VectorNorm( vect ) );
// Check if the computed value is in the desired range.
error = ( sd < min || sd > max );
}
// If not, signal the error to the subject.
// Here I take the approach of calling a method to signal the error.
// We agree instead that the routine should either return a null pointer
// if there is no error, or return the error message as a static string.
if ( error ) {
// If the user provided a message to signal a visibilty error, use it.
// If not, generate a generic message.
if ( !msg ) msg = "Movement extent outside range.";
// The message could be different depending on whether the maniplulandum
// was not moved enough, or if it just could not be seen.
if ( N <= 0.0 ) fmt = "%s\n Manipulandum not visible.";
else fmt = "%s\n Measured: %f\n Desired range: %f - %f\n Direction: < %.2f %.2f %.2f>";
int response = fSignalError( MB_ABORTRETRYIGNORE, picture, fmt, msg, sd, min, max, dirX, dirY, dirZ );
if ( response == IDABORT ) return( ABORT_EXIT );
if ( response == IDRETRY ) return( RETRY_EXIT );
if ( response == IDIGNORE ) return( IGNORE_EXIT );
}
// This is my means of signalling the event.
monitor->SendEvent( "Movement extent OK.\n Measured: %f\n Desired range: %f - %f\n Direction: < %.2f %.2f %.2f>", sd, min, max, dirX, dirY, dirZ );
return( NORMAL_EXIT );
}
