void Basker<Int,Entry,Exe_Space>::DEBUG_PRINT()
{
//print_sep_bal();
#ifdef BASKER_2DL
printL2D();
printLMTX();
#else
//printL();
#endif
std::cout << "L printed " << std::endl;
printU();
printUMTX();
std::cout << "U printed" << std::endl;
//printRHS();
std::cout << "RHS printed" << std::endl;
//printSOL();
std::cout << "SOL printed" << std::endl;
//printTree();
std::cout << "Tree printed" << std::endl;
//Print out vectors
if(match_flag == BASKER_TRUE)
{
printVec("match.csc", order_match_array,
order_match_array.dimension_0());
}
if(btf_flag == BASKER_TRUE)
{
printVec("btf.csc", order_btf_array,
order_btf_array.dimension_0());
printVec("amdblk.csc", order_blk_amd_array,
order_blk_amd_array.dimension_0());
}
if(btf_tabs_offset != 0)
{
printVec("ND.csc", part_tree.permtab,
part_tree.permtab.dimension_0());
}
if(amd_flag == BASKER_TRUE)
{
printVec("camd.csc", order_csym_array,
order_csym_array.dimension_0());
}
}//end DEBUG_PRINT()
int main( int argn, char *arg[] )
{
srand( 1234567 );
omp_set_num_threads(2);
std::cout << "Number columns: " << N_COL << std::endl;
std::cout << "Number rows: " << N_ROW << std::endl;
std::cout << "Total Number Values: " << N_ROW * N_COL << std::endl;
std::cout << "Number Non-Zeros: " << NNZ << std::endl;
std::cout << "Max Number Threads: " << omp_get_max_threads() << std::endl;
// allocate some memory
// ... for matrix and fill it with random values
double Aval[NNZ] = {1.0, 3.0, 4.0, 2.0, 5.0, 2.0, 1.0};
int AcolInd[NNZ] = {1, 3, 2, 0, 4, 2, 3};
int ArowPt[N_ROW+1] = {0, 2, 3, 5, 6};
// ... for vector and fill it with random, non-zero, values
double Vval[N_COL] = {1.0, 3.0, 4.0, 2.0, 3.0};
// ... for result and make sure, it's zero everywhere
double Yval[N_ROW];
for ( int i = 0; i < N_ROW; i++ ) {
Yval[i] = 0;
}
// print input if dimenions not too high
if ( N_COL < 10 && N_ROW < 10 && NNZ < 15 ) {
printSparseMat( Aval, AcolInd, ArowPt );
printVec( Vval, N_COL );
}
// multiply --- here we go!
mxv( Aval, AcolInd, ArowPt, Vval, Yval );
// print result if dimenions not too high
if ( N_ROW < 10 ) {
printVec( Yval, N_ROW );
}
// print squared norm of solution vector as a measurement for correctness
double sqnorm = 0;
for ( int i = 0; i < N_ROW; i++ ) {
sqnorm += Yval[i] * Yval[i];
}
std::cout << "Squared Norm of Y is: " << sqnorm << std::endl;
}
void cross4h(double u[], double v0[], double v1[]){
u[0] = v0[1] * v1[2] - v0[2] * v1[1];
u[1] = v0[2] * v1[0] - v0[0] * v1[2];
u[2] = v0[0] * v1[1] - v0[1] * v1[0];
#ifdef DEBUG
printf("外積:");
printVec(u);
#endif
}
int main(int argc, char** argv) {
/* Declarations for calculations */
// Dynamically generated vectors
int p;
int my_rank;
int i,j;
double tempD;
int tempI;
char * pEnd;
int n = 12;//atoi(argv[1]);
MPI_Init(&argc, &argv);
MPI_Comm_rank(MPI_COMM_WORLD, &my_rank);
MPI_Comm_size(MPI_COMM_WORLD, &p);
int nDivP = n/p;
double avg;
int myAbove = 0;
int above;
srand(time(NULL) - my_rank);
double sum;
double mySum;
double* IQvec = malloc(sizeof(double)*nDivP);
for (i = 0; i < nDivP; i++) {
IQvec[i] = rand() % 20 + 90;
mySum += IQvec[i];
}
printVec(IQvec,nDivP,my_rank);
//
MPI_Reduce(&mySum,&sum,1,MPI_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD);
MPI_Bcast(&sum,1,MPI_DOUBLE,0,MPI_COMM_WORLD);
avg = sum / n;
// Everybody
for (j = 0; j < nDivP; j++)
if (IQvec[j] > avg) myAbove++;
MPI_Reduce(&myAbove,&above,1,MPI_INT,MPI_SUM,0,MPI_COMM_WORLD);
if (my_rank == 0)
printf("\nSum = %lf. Above = %d. Avg = %lf \n\n",sum, above, avg);
free(IQvec);
MPI_Finalize();
return 0;
}
void loop() {
//Variable Initialization
time ++;
api.getMyZRState(myState);
api.getOtherZRState(otherState);
phase = game.getCurrentPhase();
fuel = game.getFuelRemaining();
myScore = game.getScore();
otherScore = game.getOtherScore();
dust = game.getRemainingMaterial();
chg = game.getCharge();
if (time < 2) {
red = statePos(myState)[0] < floatZero;
}
//Debug Console
DEBUG(("\nDevilTech Message Screen: Fall 2012\n"));
#if printDebug
DEBUG(("Debug Info\n"));
DEBUG(("\tTime (s)\t%d\tPhase\t%d\tPlayer\t%s\n", time, phase, (red ? "Red" : "Blue")));
DEBUG(("\tFuel\t%.3f\tDust\t%.3f\tChg\t%d\n", fuel, dust, chg));
DEBUG(("\tScore\t%.2f\tOther Score\t%.2f\n", myScore, otherScore));
DEBUG(("\tOther Message\t%u\n", otherMessage));
printVec("\tMy Pos (m)", statePos(myState));
printVec("\tMy Vel (m/s)", stateVel(myState));
printVec("\tMy Att (unitvec)", stateAtt(myState));
printVec("\tMy AngVel (rad/s)", stateAngVel(myState));
printVec("\tOther Pos (m)", statePos(otherState));
printVec("\tOther Vel (m/s)", stateVel(otherState));
printVec("\tOther Att (unitvec)", stateAtt(otherState));
printVec("\tOther AngVel (rad/s)", stateAngVel(otherState));
#endif
//Game Logic
vec temp;
game.getItemLocation(2, temp);
if (!game.haveObject(0)) getItem(0);/*
else if (!game.haveObject(1) && !game.otherHasObject(1) && count2 < 25) {
getItem(1);
count2 ++;
}*/
else if (!game.haveObject(2) && !game.otherHasObject(2) && disBetweenPts(otherState, temp) > 0.075f && count3 < 25) {
getItem(2);
count3 ++;
}
else {
smartGo(red ? 0.055f : -0.055f, -0.065f);
if (phase == 3) removeObs();
}
}
int main() {
std::vector<int> vec { -10, -1, 0, 3, 10, 11, 30, 50, 100 };
printVec(vec);
int fixedPoint = binarySearchFixedPoint(vec);
if (fixedPoint != -1) {
std::cout << "Above vector has a fixed point at index(0 indexed): " << fixedPoint << std::endl;
} else {
std::cout << "Above vector has no fixed point\n";
}
return 0;
}
int main()
{
vector<int> vec;
for (int i = 0; i < 10; ++i)
{
vec.push_back(i);
}
printVec(vec);
for (vector<int>::iterator iter = vec.begin();
iter != vec.end(); ++iter)
{
*iter *= 2;
}
printVec(vec);
cout << vec.end() - vec.begin() << endl;
//vector<int>::iterator iter2 = (vec.begin() + vec.end())/2;
return 0;
}
//END::PAGE::adef
//BEGIN::PAGE::ctrl
void overshoot(vec targetPos) {
vec nPos, att;
float dis = disBetweenPts(statePos(myState), targetPos);
attToTarget(statePos(myState), targetPos, att);
nPos[0] = targetPos[0] + att[0] * osMag * dis; //set target location beyond target
nPos[1] = targetPos[1] + att[1] * osMag * dis;
nPos[2] = targetPos[2] + att[2] * osMag * dis;
float velComponent = mathVecInner(stateVel(myState), att, 3);
float disLeft = dis - stillDis;
float velDis = disToHalt(velComponent);
#if printDebug
DEBUG(("Overshoot Info\n"));
DEBUG(("\tOvershoot Magnitude\t%.3f\n", osMag));
DEBUG(("\tOvershoot Threshold\t%.3f\n", lim));
DEBUG(("\tDis to Target (m)\t%.3f\n", dis));
DEBUG(("\tVel Component (m/s)\t%.3f\n", velComponent));
DEBUG(("\tCur Vel Dis (m)\t%.3f\n", velDis));
printVec("\tTarget Pos (m)", targetPos);
printVec("\tTarget Att (unitvec)", att);
printVec("\tNew Pos (m)", nPos);
#endif
if (osMag <= floatZero || hitTarget(myState, targetPos)) {
#if printDebug
DEBUG(("\tsetPositionTarget\n"));
#endif
api.setPositionTarget(targetPos);
}
else if (dis < lim && velDis > disLeft + 0.001f) {
#if printDebug
DEBUG(("\tHalting\n"));
#endif
api.setVelocityTarget(zeroVec);
}
else { //overshoot
#if printDebug
DEBUG(("\tSpeeding Up\n"));
#endif
api.setPositionTarget(nPos);
}
}
static void test_lemke()
{
real * A = makeRandMatrix();
real * b = makeRandVec2();
real * x = (real *)malloc(2*NN*sizeof(real));
real * xx = (real *)malloc(2*NN*sizeof(real));
real * AA = makeMatrix();
real * bb = makeRandVec2();
std::vector<real *> vx;
// copyMatrix(A,exaples_m2);
// for(int i =0;i<N;i++)
// b[i] = exaples_q2[i];
copyMatrix(AA,A);
memcpy(bb,b,NN*sizeof(real));
//memset(x,0,sizeof(real)*N);
memset(x,0,NN*sizeof(real));
//memcpy(xx,b,N*sizeof(real));
memset(xx,0,sizeof(real)*NN);
printf("--------------------------------------------------------\n");
printMat("solve lcp A=",A);
printVec("lcp b=",b);
printf("--------------------------------------------------------\n");
int result1 = lcp(A,b,vx,NN);
int result2 = lcp_lemke(A,b,x,NN);
printf("lcp solve:\n");
printf("--------------------------------------------------------\n");
printLCPVx(vx);
printf("lcp_lemke solve %s (%d)\n",result2?"true":"false",result2);
printVec("lcp_lemke=",x);
check_lcp_result(AA,bb,x,NN);
freeMatrix(A);
freeMatrix(b);
freeMatrix(x);
freeMatrix(AA);
freeMatrix(bb);
freeLcpSolve(vx);
}
int main()
{
vector<int> nums;
int i=0;
for(;i<15;i++)
nums.push_back(rand()%100);
vector<vector<int> >res=subsets(nums);
for(i=0;i<(signed)res.size();i++)
printVec(res[i]);
return 0;
}
static void test_pgs()
{
real * A = makeRandSPDMatrix();
real * b = makeRandVec2();
real * x = makeRandVec2();
real * xx = makeRandVec2();
real * AA = makeMatrix();
real * bb = makeRandVec2();
std::vector<real *> vx;
copyMatrix(AA,A);
memcpy(bb,b,NN*sizeof(real));
//memset(x,0,sizeof(real)*N);
memset(x,0,NN*sizeof(real));
//memcpy(xx,b,N*sizeof(real));
memset(xx,0,sizeof(real)*NN);
printf("--------------------------------------------------------\n");
printMat("solve lcp A=",A);
printVec("lcp b=",b);
printf("--------------------------------------------------------\n");
int result1 = lcp(A,b,vx,NN);
int result2 = lcp_pgs(A,b,x,NN,15,0.001);
int result3 = Solve_GaussSeidel(AA,bb,xx,NN,15);
printf("lcp solve:\n");
printf("--------------------------------------------------------\n");
printLCPVx(vx);
printf("lcp_pgs solve %s (%d)\n",result2?"true":"false",result2);
printVec("lcp_pgs=",x);
check_lcp_result(AA,bb,x,NN);
printVec("gs solve :",xx);
freeMatrix(A);
freeMatrix(b);
freeMatrix(x);
freeMatrix(AA);
freeMatrix(bb);
freeLcpSolve(vx);
}
BASKER_INLINE
int Basker<Int,Entry,Exe_Space>::serial_backward_solve
(
ENTRY_1DARRAY y,
ENTRY_1DARRAY x
)
{
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("called serial backward solve \n");
#endif
for(Int b = tree.nblks-1; b >=0; b--)
{
//printf("--HERE-\n");
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Upper solve blk: %d \n", b);
#endif
BASKER_MATRIX &U = LU(b)(LU_size(b)-1);
//printf("\n--------U------------\n");
//U.print();
//U\y -> x
upper_tri_solve(U,y,x);
for(Int bb = LU_size(b)-2; bb >= 0; bb--)
{
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Upper solver spmv: %d %d \n",
b, bb);
#endif
BASKER_MATRIX &UB = LU(b)(bb);
//y = UB*x;
neg_spmv(UB,x,y);
}
}//end over all blks
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Done with Upper Solve: \n");
printVec(x, gn);
#endif
return 0;
}//end serial_backward_solve()
void testRotsNShifts(const FHEPubKey& publicKey,
const FHESecKey& secretKey,
const EncryptedArrayCx& ea, double epsilon)
{
std::srand(std::time(0)); // set seed, current time.
int nplaces = rand() % static_cast<int>(ea.size()/2.0) + 1;
if (verbose)
cout << "Test Rotation of " << nplaces << ": ";
Ctxt c1(publicKey);
vector<cx_double> vd1;
vector<cx_double> vd_dec;
ea.random(vd1);
ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
#ifdef DEBUG_PRINTOUT
printVec(cout<< "vd1=", vd1, 10)<<endl;
#endif
std::rotate(vd1.begin(), vd1.end()-nplaces, vd1.end());
ea.rotate(c1, nplaces);
ea.decrypt(c1, secretKey, vd_dec);
#ifdef DEBUG_PRINTOUT
printVec(cout<< "vd1(rot)=", vd1, 10)<<endl;
printVec(cout<<"res: ", vd_dec, 10)<<endl;
#endif
if (cx_equals(vd1, vd_dec, conv<double>(epsilon*c1.getPtxtMag())))
cout << "GOOD\n";
else {
cout << "BAD:\n";
std::cout << " max(vd)="<<largestCoeff(vd_dec)
<< ", max(vd1)="<<largestCoeff(vd1)
<< ", maxDiff="<<calcMaxDiff(vd_dec,vd1) << endl<<endl;
}
}
bool makeObs (vec loc) {
#if printDebug
DEBUG(("makeObs()\n"));
printVec("\tAt loc (m):", loc);
#endif
if (!cloudOn) {
targetAtt[0] = -myState[6];
targetAtt[1] = -myState[7];
targetAtt[2] = -myState[8];
game.startObstacle();
cloudOn = true;
}
else if (hitTarget(myState, loc)) {
game.stopObstacle();
cloudOn = false;
return true;
}
#if printDebug
printVec("\tTarget att", targetAtt);
#endif
api.setAttitudeTarget(targetAtt);
api.setPositionTarget(loc);
return false;
}
BASKER_INLINE
int Basker<Int,Entry,Exe_Space>::serial_forward_solve
(
ENTRY_1DARRAY y,
ENTRY_1DARRAY x
)
{
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Called serial forward solve \n");
#endif
//Forward solve on A
for(Int b = 0; b < tree.nblks; ++b)
//for(Int b = 0; b < 1; ++b)
{
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Lower Solve blk: %d \n",b);
#endif
BASKER_MATRIX &L = LL(b)(0);
//L\y -> x
lower_tri_solve(L, y, x);
//Update offdiag
for(Int bb = 1; bb < LL_size(b); ++bb)
{
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Lower Solver Update blk: %d %d \n",
b, bb);
#endif
BASKER_MATRIX &LD = LL(b)(bb);
//y = LD*x;
neg_spmv_perm(LD, x, y);
}
//printf("spmv test\n");
//printVec(y,gn);
}
#ifdef BASKER_DEBUG_SOLVE_RHS
printf("Done forward solve A \n");
printVec(x, gn);
#endif
return 0;
}//end serial_forward_solve()
void setVelocity (vec targetVel) {
#if printDebug
DEBUG(("Velocity Targeting Info\n"));
printVec("Target Vel (m)", targetVel);
#endif
vec temp;
mathVecSubtract(temp, targetVel, stateVel(myState), 3);
float magnitude = mathVecMagnitude(temp, 3);
if (magnitude < 0.02f) {
api.setVelocityTarget(targetVel);
return;
}
vecScale(temp, 4.2f);
magnitude = fabs(mathVecMagnitude(temp, 3));
if (magnitude > 0.11f) vecScale(temp, 0.11f / magnitude);
api.setForces(temp);
}
void getItem (int itemNo) {
#if printDebug
DEBUG(("getItem(%d) Info\n", itemNo));
#endif
vec iloc;
game.getItemLocation(itemNo, iloc);
#if printDebug
printVec("\tItemPos (m)", iloc);
#endif
api.setPositionTarget(iloc);
if (hitTarget(myState, iloc)) {
vec temp = {0.75f, floatZero, floatZero};
setTargetAngVel(temp);
}
else {
setTargetAngVel(zeroVec);
}
}
int main(int argc, char** argv)
{
printVec(glm::dvec3(1, 2, 3));
//init GLUT and create windows
glutInit(&argc, argv);
glutInitWindowSize(1024, 768);
glutInitWindowPosition(500, 110);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutCreateWindow("SPH");
init();
glutReshapeFunc(resize);
glutDisplayFunc(display);
glutSpecialFunc(processSpecialKeys);
glutIdleFunc(idle);
// enter GLUT event processing cycle
glutMainLoop();
}
void getItem (int itemNo) {
#if printDebug
DEBUG(("getItem(%d) Info\n", itemNo));
#endif
if (game.getCurrentPhase() == 1) {
smartGo(red ? -0.45 : 0.45, statePos(myState)[1] < -0.05f ? 0.073f : 0.0505f);
#if printDebug
DEBUG(("\tMoving to Zone 2\n"));
#endif
return;
}
vec iloc;
game.getItemLocation(itemNo, iloc);
#if printDebug
printVec("\tItemPos (m)", iloc);
#endif
overshoot(iloc);
vec degs = {floatZero, floatZero, 0.62f};
setTargetAngVel(hitTarget(myState, iloc) ? degs : zeroVec);
}
void remDust () {
int i = obsDetected = game.getIdentifiedObstacles(obstacles);
api.setAttitudeTarget(makeVec(floatZero, -1.0f, floatZero));
#if printDebug
DEBUG(("remDust()\n"));
DEBUG(("\tDetected\t%d\n", obsDetected));
#endif
while (i -- && chg) {
#if printDebug
DEBUG(("\tObstacle ID\t%d\tRadius\t%.3f\n", obstacles[i].ID, obstacles[i].size));
printVec("\t\tObstacle Location", obstacles[i].loc);
#endif
if (obstacles[i].visible) {
#if printDebug
DEBUG(("\t\tShrinking\n"));
#endif
game.shrinkObstacle(obstacles[i].ID);
return;
}
}
}
void getItem (int itemNo, float x0, float x1, float y0, float y1, float z0, float z1, float acc) {
float best = floatZero;
int lim = 40;
if (time < lim)
lim = time;
for (float x = x0; x < x1 + floatItsml; x += acc) {
for (float y = y0; y < y1 + floatItsml; y += acc) {
for (float z = z0; z < z1 + floatItsml; z += acc) {
float temp = floatZero;
for (int i = 0; i <= lim; i ++) {
float diff = fabs(tridis[i][itemNo] - disBetweenPts(makeVec(x, y, z), tripos[i]));
if (diff < 0.0101f)
temp += floatOne - diff;
}
if (temp > best) {
iloc[0] = x; iloc[1] = y; iloc[2] = z;
best = temp;
}
}
}
}
#if printDebug
DEBUG(("getItem(%d)\n", itemNo));
printVec("\tItemPos (m)", iloc);
DEBUG(("\tAccuracy Score\t%.3f\n", best / (time + 1.0f)));
DEBUG(("\tVel Magnitude (m/s)\t%.3f\n", mathVecMagnitude(myState + 3, 3)));
DEBUG(("\tAngvel Magnitude (deg/s)\t%.3f\n", mathVecMagnitude(myState + 9, 3) * 57.296f));
DEBUG(("\tDis from Item (m)\t%.3f\n", disBetweenPts(myState, iloc)));
#endif
api.setPositionTarget(iloc);
if (hitTarget(myState, iloc)) {
#if printDebug
DEBUG(("\tHit target\n"));
#endif
setTargetAngVel(makeVec(floatZero, floatZero, pickUpSpeed));
}
else {
setTargetAngVel(zeroVec);
}
}
glm::mat4 lookAt4D(const glm::vec4 &from, const glm::vec4 &to,
const glm::vec4 &up, const glm::vec4 &over)
{
glm::mat4 viewMat;
glm::vec4 A, B, C, D;
D = glm::normalize(from - to);
std::cout << "D ";
printVec(D, 4);
std::cout << "A ";
A = glm::normalize(cross4D(up, over, D));
std::cout << "B ";
B = glm::normalize(cross4D(over, D, A));
std::cout << "C ";
C = cross4D(D, A, B);
viewMat[0] = A;
viewMat[1] = B;
viewMat[2] = C;
viewMat[3] = D;
// I hope this preserves "right-handedness"
//assert(glm::normalize(cross4D(A, B, C)) == D);
return glm::transpose(viewMat);
}
void NeuralNetwork::addAllTrainData(vector <Vec > ins, vector< Vec> outs){
double maxError = 100000 ;
double meanError = 100000;
double prevError =0;
Vec outp;
int printRate = PRINTRATE;
int steps=0;
while(meanError>ERROR_THRESHOLD && steps!=MAXSTEPS){
meanError = 0;
maxError = 0;
steps++;
for(int i =0;i< ins.size();i++){
addTrainData(ins[i],outs[i]);
outp = layers[noOfLayers-1].getOutput();
sub(outp , outs[i]);
maxError = max(maxError , mod(outp));
meanError += mod(outp);
//cout<<mod(outp)<<" "<<endl;
}
if(!printRate--){
printf("%d mean:%f max:%f\n",steps, meanError , maxError);
printVec(outp);
cout<<endl;
printRate =PRINTRATE;
}
if(meanError<prevError)
NETA = NETA*1.2;
else
NETA = NETA/2;
prevError = meanError;
}
printf("Training Done Error %f \n",meanError);
printf("Total steps : %d\n",steps);
printf("\n\n");
}
// FUNCTION ====== setup
void setup() {
//---- SDL initialization
if ( SDL_Init(SDL_INIT_VIDEO) != 0 ) {
printf("Unable to initialize SDL: %s\n", SDL_GetError());
getchar() ;
}
SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
//screen = SDL_SetVideoMode( WIDTH, HEIGHT, COLDEPTH, SDL_OPENGL | SDL_SWSURFACE );
screen = SDL_SetVideoMode( WIDTH, HEIGHT, COLDEPTH, SDL_OPENGL );
if(!screen) {
printf("Couldn't set %dx%d GL video mode: %s\n", WIDTH,HEIGHT, SDL_GetError());
SDL_Quit();
exit(2);
}
SDL_WM_SetCaption(" Prokop's test SDL+ OpenGL", "SDL+ OpenGL");
//materialy
float ambient [] = { 0.25f, 0.25f, 0.25f, 1.0f};
glMaterialfv (GL_FRONT_AND_BACK, GL_AMBIENT, ambient);
float diffuse [] = { 0.50f, 0.50f, 0.50f, 1.0f};
glMaterialfv (GL_FRONT_AND_BACK, GL_DIFFUSE, diffuse);
float specular [] = { 0.00f, 0.00f, 0.00f, 1.0f};
glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, specular);
float shininess[] = { 0.0f };
glMaterialfv (GL_FRONT_AND_BACK, GL_SHININESS, shininess);
glEnable (GL_LIGHT0);
float light0_pos [] = { 1000.0f, 2000.0f, 3000.0f, 1.00f};
glLightfv (GL_LIGHT0, GL_POSITION, light0_pos );
float light0_color[] = { 0.9f, 0.8f, 0.7f, 1.00f};
glLightfv (GL_LIGHT0, GL_DIFFUSE, light0_color );
float light0_ambient[] = { 0.5f, 0.6f, 0.7f, 1.00f};
glLightfv (GL_LIGHT0, GL_AMBIENT, ambient );
glEnable (GL_LIGHTING);
glEnable (GL_COLOR_MATERIAL);
glEnable (GL_NORMALIZE);
glEnable (GL_DEPTH_TEST);
//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//glEnable(GL_BLEND) ;
//glBlendFunc (GL_SRC_ALPHA, GL_SRC_ALPHA);
glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
glShadeModel(GL_SMOOTH);
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
glClearColor( 0.9, 0.9, 0.9, 0.0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
qmouse.setOne();
srand(1234);
buildings = makeBuildings( 10, 10, 100, 100, 0.5, 0.5, 50, 100 );
const int len = 5;
double masses[len] = { 4,1,1,0.5f,0.5f };
Vec3d poss[len] = { {0,-0.2,1}, {-2,0,0}, {2,0,0}, {0,0,-3}, {0,0.2,-3} };
myCraft.from_mass_points( len, masses, poss );
myCraft.qrot.setOne();
//myCraft.qrot.set(0,0,-0.5,1); myCraft.qrot.normalize();
printVec(myCraft.pos);
printf("pos\n");
myCraft.wingLeft .craft = &myCraft;
myCraft.wingRight.craft = &myCraft;
myCraft.elevator .craft = &myCraft;
myCraft.rudder .craft = &myCraft;
myCraft.wingLeft .lpos.set( poss[1] - myCraft.pos );
myCraft.wingRight.lpos.set( poss[2] - myCraft.pos );
myCraft.elevator .lpos.set( poss[3] - myCraft.pos );
myCraft.rudder .lpos.set( poss[4] - myCraft.pos );
/*
myCraft.wingLeft .lpos.set( poss[1] );
myCraft.wingRight.lpos.set( poss[2] );
myCraft.elevator .lpos.set( poss[3] );
myCraft.rudder .lpos.set( poss[4] );
*/
myCraft.wingLeft .lrot.set( { 1,0,0, 0,1,0, 0,0,1 } );
myCraft.wingLeft .lrot.rotate( -0.1, { 0,0,1 } );
myCraft.wingRight.lrot.set( { 1,0,0, 0,1,0, 0,0,1 } );
myCraft.wingRight .lrot.rotate( +0.1, { 0,0,1 } );
myCraft.elevator .lrot.set( { 1,0,0, 0,1,0, 0,0,1 } );
myCraft.elevator .lrot.rotate( +0.2, { 1,0,0 } );
printf("elevator lrot\n");
printMat( myCraft.elevator.lrot );
myCraft.rudder .lrot.set( { 0,1,0, 1,0,0, 0,0,1 } );
myCraft.wingLeft .C.set( 0.05, 1.0, 0.05 );
myCraft.wingRight.C.set( 0.05, 1.0, 0.05 );
myCraft.elevator .C.set( 0.05, 1.0, 0.05 );
myCraft.elevator.C *= 0.1;
myCraft.rudder .C.set( 0.05, 1.0, 0.05 );
myCraft.rudder .C *= 0.1;
myCraft.L.set(0,0,0);
myCraft.init( );
myCraft.vel.set(0,0,0);
myCraft.pos.set(0,500,0);
printf("Ibody\n");
printMat(myCraft.Ibody);
printf("invIbody\n");
printMat(myCraft.invIbody);
}
ostream& printZZX(ostream& s, const ZZX& poly, long nCoeffs)
{
return printVec(s, poly.rep, nCoeffs);
}
SEXP spMisalign(SEXP Y_r, SEXP X_r, SEXP p_r, SEXP n_r, SEXP m_r, SEXP coordsD_r,
SEXP betaPrior_r, SEXP betaNorm_r,
SEXP KPrior_r, SEXP KPriorName_r,
SEXP PsiPrior_r,
SEXP nuUnif_r, SEXP phiUnif_r,
SEXP phiStarting_r, SEXP AStarting_r, SEXP PsiStarting_r, SEXP nuStarting_r,
SEXP phiTuning_r, SEXP ATuning_r, SEXP PsiTuning_r, SEXP nuTuning_r,
SEXP nugget_r, SEXP covModel_r, SEXP amcmc_r, SEXP nBatch_r, SEXP batchLength_r, SEXP acceptRate_r, SEXP verbose_r, SEXP nReport_r){
/*****************************************
Common variables
*****************************************/
int h, i, j, k, l, b, s, ii, jj, kk, info, nProtect= 0;
char const *lower = "L";
char const *upper = "U";
char const *nUnit = "N";
char const *yUnit = "U";
char const *ntran = "N";
char const *ytran = "T";
char const *rside = "R";
char const *lside = "L";
const double one = 1.0;
const double negOne = -1.0;
const double zero = 0.0;
const int incOne = 1;
/*****************************************
Set-up
*****************************************/
double *Y = REAL(Y_r);
double *X = REAL(X_r);
int *p = INTEGER(p_r);
int *n = INTEGER(n_r);
int m = INTEGER(m_r)[0];
int nLTr = m*(m-1)/2+m;
int N = 0;
int P = 0;
for(i = 0; i < m; i++){
N += n[i];
P += p[i];
}
int mm = m*m;
int NN = N*N;
int NP = N*P;
int PP = P*P;
double *coordsD = REAL(coordsD_r);
std::string covModel = CHAR(STRING_ELT(covModel_r,0));
//priors
std::string betaPrior = CHAR(STRING_ELT(betaPrior_r,0));
double *betaMu = NULL;
double *betaC = NULL;
if(betaPrior == "normal"){
betaMu = (double *) R_alloc(P, sizeof(double));
F77_NAME(dcopy)(&P, REAL(VECTOR_ELT(betaNorm_r, 0)), &incOne, betaMu, &incOne);
betaC = (double *) R_alloc(PP, sizeof(double));
F77_NAME(dcopy)(&PP, REAL(VECTOR_ELT(betaNorm_r, 1)), &incOne, betaC, &incOne);
}
double *phiUnif = REAL(phiUnif_r);
std::string KPriorName = CHAR(STRING_ELT(KPriorName_r,0));
double KIW_df = 0; double *KIW_S = NULL;
double *ANormMu = NULL; double *ANormC = NULL;
if(KPriorName == "IW"){
KIW_S = (double *) R_alloc(mm, sizeof(double));
KIW_df = REAL(VECTOR_ELT(KPrior_r, 0))[0]; KIW_S = REAL(VECTOR_ELT(KPrior_r, 1));
}else{//assume A normal (can add more specifications later)
ANormMu = (double *) R_alloc(nLTr, sizeof(double));
ANormC = (double *) R_alloc(nLTr, sizeof(double));
for(i = 0; i < nLTr; i++){
ANormMu[i] = REAL(VECTOR_ELT(KPrior_r, 0))[i];
ANormC[i] = REAL(VECTOR_ELT(KPrior_r, 1))[i];
}
}
bool nugget = static_cast<bool>(INTEGER(nugget_r)[0]);
double *PsiIGa = NULL; double *PsiIGb = NULL;
if(nugget){
PsiIGa = (double *) R_alloc(m, sizeof(double));
PsiIGb = (double *) R_alloc(m, sizeof(double));
for(i = 0; i < m; i++){
PsiIGa[i] = REAL(VECTOR_ELT(PsiPrior_r, 0))[i];
PsiIGb[i] = REAL(VECTOR_ELT(PsiPrior_r, 1))[i];
}
}
//matern
double *nuUnif = NULL;
if(covModel == "matern"){
nuUnif = REAL(nuUnif_r);
}
bool amcmc = static_cast<bool>(INTEGER(amcmc_r)[0]);
int nBatch = INTEGER(nBatch_r)[0];
int batchLength = INTEGER(batchLength_r)[0];
double acceptRate = REAL(acceptRate_r)[0];
int nSamples = nBatch*batchLength;
int verbose = INTEGER(verbose_r)[0];
int nReport = INTEGER(nReport_r)[0];
if(verbose){
Rprintf("----------------------------------------\n");
Rprintf("\tGeneral model description\n");
Rprintf("----------------------------------------\n");
Rprintf("Model fit with %i outcome variables.\n\n", m);
Rprintf("Number of observations within each outcome:"); printVec(n, m);
Rprintf("\nNumber of covariates for each outcome (including intercept if specified):"); printVec(p, m);
Rprintf("\nTotal number of observations: %i\n\n", N);
Rprintf("Total number of covariates (including intercept if specified): %i\n\n", P);
Rprintf("Using the %s spatial correlation model.\n\n", covModel.c_str());
if(amcmc){
Rprintf("Using adaptive MCMC.\n\n");
Rprintf("\tNumber of batches %i.\n", nBatch);
Rprintf("\tBatch length %i.\n", batchLength);
Rprintf("\ttarget acceptance rate %.5f.\n", acceptRate);
Rprintf("\n");
}else{
Rprintf("Number of MCMC samples %i.\n\n", nSamples);
}
if(!nugget){
Rprintf("Psi not included in the model (i.e., no nugget model).\n\n");
}
Rprintf("Priors and hyperpriors:\n");
if(betaPrior == "flat"){
Rprintf("\tbeta flat.\n");
}else{
Rprintf("\tbeta normal:\n");
Rprintf("\tmu:"); printVec(betaMu, P);
Rprintf("\tcov:\n"); printMtrx(betaC, P, P);
}
Rprintf("\n");
if(KPriorName == "IW"){
Rprintf("\tK IW hyperpriors df=%.5f, S=\n", KIW_df);
printMtrx(KIW_S, m, m);
}else{
Rprintf("\tA Normal hyperpriors\n");
Rprintf("\t\tparameter\tmean\tvar\n");
for(j = 0, i = 0; j < m; j++){
for(k = j; k < m; k++, i++){
Rprintf("\t\tA[%i,%i]\t\t%3.1f\t%1.2f\n", j+1, k+1, ANormMu[i], ANormC[i]);
}
}
}
Rprintf("\n");
if(nugget){
Rprintf("\tDiag(Psi) IG hyperpriors\n");
Rprintf("\t\tparameter\tshape\tscale\n");
for(j = 0; j < m; j++){
Rprintf("\t\tPsi[%i,%i]\t%3.1f\t%1.2f\n", j+1, j+1, PsiIGa[j], PsiIGb[j]);
}
}
Rprintf("\n");
Rprintf("\tphi Unif hyperpriors\n");
Rprintf("\t\tparameter\ta\tb\n");
for(j = 0; j < m; j++){
Rprintf("\t\tphi[%i]\t\t%0.5f\t%0.5f\n", j+1, phiUnif[j*2], phiUnif[j*2+1]);
}
Rprintf("\n");
if(covModel == "matern"){
Rprintf("\tnu Unif hyperpriors\n");
for(j = 0; j < m; j++){
Rprintf("\t\tnu[%i]\t\t%0.5f\t%0.5f\n", j+1, nuUnif[j*2], nuUnif[j*2+1]);
}
Rprintf("\n");
}
}
/*****************************************
Set-up MCMC sample matrices etc.
*****************************************/
//spatial parameters
int nParams, AIndx, PsiIndx, phiIndx, nuIndx;
if(!nugget && covModel != "matern"){
nParams = nLTr+m;//A, phi
AIndx = 0; phiIndx = nLTr;
}else if(nugget && covModel != "matern"){
nParams = nLTr+m+m;//A, diag(Psi), phi
AIndx = 0; PsiIndx = nLTr; phiIndx = PsiIndx+m;
}else if(!nugget && covModel == "matern"){
nParams = nLTr+2*m;//A, phi, nu
AIndx = 0; phiIndx = nLTr, nuIndx = phiIndx+m;
}else{
nParams = nLTr+3*m;//A, diag(Psi), phi, nu
AIndx = 0; PsiIndx = nLTr, phiIndx = PsiIndx+m, nuIndx = phiIndx+m;
}
double *params = (double *) R_alloc(nParams, sizeof(double));
//starting
covTrans(REAL(AStarting_r), ¶ms[AIndx], m);
if(nugget){
for(i = 0; i < m; i++){
params[PsiIndx+i] = log(REAL(PsiStarting_r)[i]);
}
}
for(i = 0; i < m; i++){
params[phiIndx+i] = logit(REAL(phiStarting_r)[i], phiUnif[i*2], phiUnif[i*2+1]);
if(covModel == "matern"){
params[nuIndx+i] = logit(REAL(nuStarting_r)[i], nuUnif[i*2], nuUnif[i*2+1]);
}
}
//tuning and fixed
double *tuning = (double *) R_alloc(nParams, sizeof(double));
int *fixed = (int *) R_alloc(nParams, sizeof(int)); zeros(fixed, nParams);
for(i = 0; i < nLTr; i++){
tuning[AIndx+i] = REAL(ATuning_r)[i];
if(tuning[AIndx+i] == 0){
fixed[AIndx+i] = 1;
}
}
if(nugget){
for(i = 0; i < m; i++){
tuning[PsiIndx+i] = REAL(PsiTuning_r)[i];
if(tuning[PsiIndx+i] == 0){
fixed[PsiIndx+i] = 1;
}
}
}
for(i = 0; i < m; i++){
tuning[phiIndx+i] = REAL(phiTuning_r)[i];
if(tuning[phiIndx+i] == 0){
fixed[phiIndx+i] = 1;
}
if(covModel == "matern"){
tuning[nuIndx+i] = REAL(nuTuning_r)[i];
if(tuning[nuIndx+i] == 0){
fixed[nuIndx+i] = 1;
}
}
}
for(i = 0; i < nParams; i++){
tuning[i] = log(sqrt(tuning[i]));
}
//return stuff
SEXP samples_r, accept_r, tuning_r;
PROTECT(samples_r = allocMatrix(REALSXP, nParams, nSamples)); nProtect++;
if(amcmc){
PROTECT(accept_r = allocMatrix(REALSXP, nParams, nBatch)); nProtect++;
PROTECT(tuning_r = allocMatrix(REALSXP, nParams, nBatch)); nProtect++;
}else{
PROTECT(accept_r = allocMatrix(REALSXP, 1, nSamples/nReport)); nProtect++;
}
// /*****************************************
// Set-up MCMC alg. vars. matrices etc.
// *****************************************/
int status=1, batchAccept=0, reportCnt=0;
double logMHRatio =0, logPostCurrent = R_NegInf, logPostCand = 0, det = 0, paramsjCurrent = 0;
double Q, logDetK, SKtrace;
double *paramsCurrent = (double *) R_alloc(nParams, sizeof(double));
double *accept = (double *) R_alloc(nParams, sizeof(double)); zeros(accept, nParams);
double *C = (double *) R_alloc(NN, sizeof(double));
double *K = (double *) R_alloc(mm, sizeof(double));
double *Psi = (double *) R_alloc(m, sizeof(double));
double *A = (double *) R_alloc(mm, sizeof(double));
double *phi = (double *) R_alloc(m, sizeof(double));
double *nu = (double *) R_alloc(m, sizeof(double));
int P1 = P+1;
double *vU = (double *) R_alloc(N*P1, sizeof(double));
double *z = (double *) R_alloc(N, sizeof(double));
double *tmp_N = (double *) R_alloc(N, sizeof(double));
double *tmp_mm = (double *) R_alloc(mm, sizeof(double));
double *tmp_PP = (double *) R_alloc(PP, sizeof(double));
double *tmp_P = (double *) R_alloc(P, sizeof(double));
double *tmp_NN = NULL;
double *Cbeta = NULL;
if(betaPrior == "normal"){
tmp_NN = (double *) R_alloc(NN, sizeof(double));
Cbeta = (double *) R_alloc(NN, sizeof(double));
F77_NAME(dgemv)(ntran, &N, &P, &negOne, X, &N, betaMu, &incOne, &zero, z, &incOne);
F77_NAME(daxpy)(&N, &one, Y, &incOne, z, &incOne);
F77_NAME(dsymm)(rside, lower, &N, &P, &one, betaC, &P, X, &N, &zero, vU, &N);
F77_NAME(dgemm)(ntran, ytran, &N, &N, &P, &one, vU, &N, X, &N, &zero, tmp_NN, &N);
}
int sl, sk;
if(verbose){
Rprintf("-------------------------------------------------\n");
Rprintf("\t\tSampling\n");
Rprintf("-------------------------------------------------\n");
#ifdef Win32
R_FlushConsole();
#endif
}
GetRNGstate();
for(b = 0, s = 0; b < nBatch; b++){
for(i = 0; i < batchLength; i++, s++){
for(j = 0; j < nParams; j++){
//propose
if(amcmc){
if(fixed[j] == 1){
paramsjCurrent = params[j];
}else{
paramsjCurrent = params[j];
params[j] = rnorm(paramsjCurrent, exp(tuning[j]));
}
}else{
F77_NAME(dcopy)(&nParams, params, &incOne, paramsCurrent, &incOne);
for(j = 0; j < nParams; j++){
if(fixed[j] == 1){
params[j] = params[j];
}else{
params[j] = rnorm(params[j], exp(tuning[j]));
}
}
}
//extract and transform
covTransInvExpand(¶ms[AIndx], A, m);
for(k = 0; k < m; k++){
phi[k] = logitInv(params[phiIndx+k], phiUnif[k*2], phiUnif[k*2+1]);
if(covModel == "matern"){
nu[k] = logitInv(params[nuIndx+k], nuUnif[k*2], nuUnif[k*2+1]);
}
}
if(nugget){
for(k = 0; k < m; k++){
Psi[k] = exp(params[PsiIndx+k]);
}
}
//construct covariance matrix
sl = sk = 0;
for(k = 0; k < m; k++){
sl = 0;
for(l = 0; l < m; l++){
for(kk = 0; kk < n[k]; kk++){
for(jj = 0; jj < n[l]; jj++){
C[(sl+jj)*N+(sk+kk)] = 0.0;
for(ii = 0; ii < m; ii++){
C[(sl+jj)*N+(sk+kk)] += A[k+m*ii]*A[l+m*ii]*spCor(coordsD[(sl+jj)*N+(sk+kk)], phi[ii], nu[ii], covModel);
}
}
}
sl += n[l];
}
sk += n[k];
}
if(nugget){
sl = 0;
for(l = 0; l < m; l++){
for(k = 0; k < n[l]; k++){
C[(sl+k)*N+(sl+k)] += Psi[l];
}
sl += n[l];
}
}
if(betaPrior == "normal"){
for(k = 0; k < N; k++){
for(l = k; l < N; l++){
Cbeta[k*N+l] = C[k*N+l]+tmp_NN[k*N+l];
}
}
det = 0;
F77_NAME(dpotrf)(lower, &N, Cbeta, &N, &info); if(info != 0){error("c++ error: dpotrf failed\n");}
for(k = 0; k < N; k++) det += 2*log(Cbeta[k*N+k]);
F77_NAME(dcopy)(&N, z, &incOne, tmp_N, &incOne);
F77_NAME(dtrsv)(lower, ntran, nUnit, &N, Cbeta, &N, tmp_N, &incOne);//u = L^{-1}(y-X'beta)
Q = pow(F77_NAME(dnrm2)(&N, tmp_N, &incOne),2);
}else{//beta flat
det = 0;
F77_NAME(dpotrf)(lower, &N, C, &N, &info); if(info != 0){error("c++ error: dpotrf failed\n");}
for(k = 0; k < N; k++) det += 2*log(C[k*N+k]);
F77_NAME(dcopy)(&N, Y, &incOne, vU, &incOne);
F77_NAME(dcopy)(&NP, X, &incOne, &vU[N], &incOne);
F77_NAME(dtrsm)(lside, lower, ntran, nUnit, &N, &P1, &one, C, &N, vU, &N);//L^{-1}[v:U] = [y:X]
F77_NAME(dgemm)(ytran, ntran, &P, &P, &N, &one, &vU[N], &N, &vU[N], &N, &zero, tmp_PP, &P); //U'U
F77_NAME(dpotrf)(lower, &P, tmp_PP, &P, &info); if(info != 0){error("c++ error: dpotrf failed\n");}
for(k = 0; k < P; k++) det += 2*log(tmp_PP[k*P+k]);
F77_NAME(dgemv)(ytran, &N, &P, &one, &vU[N], &N, vU, &incOne, &zero, tmp_P, &incOne); //U'v
F77_NAME(dtrsv)(lower, ntran, nUnit, &P, tmp_PP, &P, tmp_P, &incOne);
Q = pow(F77_NAME(dnrm2)(&N, vU, &incOne),2) - pow(F77_NAME(dnrm2)(&P, tmp_P, &incOne),2) ;
}
//
//priors, jacobian adjustments, and likelihood
//
logPostCand = 0.0;
if(KPriorName == "IW"){
logDetK = 0.0;
SKtrace = 0.0;
for(k = 0; k < m; k++){logDetK += 2*log(A[k*m+k]);}
//jacobian \sum_{i=1}^{m} (m-i+1)*log(a_ii)+log(a_ii)
for(k = 0; k < m; k++){logPostCand += (m-k)*log(A[k*m+k])+log(A[k*m+k]);}
//S*K^-1
F77_NAME(dpotri)(lower, &m, A, &m, &info); if(info != 0){error("c++ error: dpotri failed\n");}
F77_NAME(dsymm)(rside, lower, &m, &m, &one, A, &m, KIW_S, &m, &zero, tmp_mm, &m);
for(k = 0; k < m; k++){SKtrace += tmp_mm[k*m+k];}
logPostCand += -0.5*(KIW_df+m+1)*logDetK - 0.5*SKtrace;
}else{
for(k = 0; k < nLTr; k++){
logPostCand += dnorm(params[AIndx+k], ANormMu[k], sqrt(ANormC[k]), 1);
}
}
if(nugget){
for(k = 0; k < m; k++){
logPostCand += -1.0*(1.0+PsiIGa[k])*log(Psi[k])-PsiIGb[k]/Psi[k]+log(Psi[k]);
}
}
for(k = 0; k < m; k++){
logPostCand += log(phi[k] - phiUnif[k*2]) + log(phiUnif[k*2+1] - phi[k]);
if(covModel == "matern"){
logPostCand += log(nu[k] - nuUnif[k*2]) + log(nuUnif[k*2+1] - nu[k]);
}
}
logPostCand += -0.5*det-0.5*Q;
//
//MH accept/reject
//
logMHRatio = logPostCand - logPostCurrent;
if(runif(0.0,1.0) <= exp(logMHRatio)){
logPostCurrent = logPostCand;
if(amcmc){
accept[j]++;
}else{
accept[0]++;
batchAccept++;
}
}else{
if(amcmc){
params[j] = paramsjCurrent;
}else{
F77_NAME(dcopy)(&nParams, paramsCurrent, &incOne, params, &incOne);
}
}
if(!amcmc){
break;
}
}//end params
/******************************
Save samples
*******************************/
F77_NAME(dcopy)(&nParams, params, &incOne, &REAL(samples_r)[s*nParams], &incOne);
R_CheckUserInterrupt();
}//end batch
//adjust tuning
if(amcmc){
for(j = 0; j < nParams; j++){
REAL(accept_r)[b*nParams+j] = accept[j]/batchLength;
REAL(tuning_r)[b*nParams+j] = tuning[j];
if(accept[j]/batchLength > acceptRate){
tuning[j] += std::min(0.01, 1.0/sqrt(static_cast<double>(b)));
}else{
tuning[j] -= std::min(0.01, 1.0/sqrt(static_cast<double>(b)));
}
accept[j] = 0.0;
}
}
//report
if(status == nReport){
if(verbose){
if(amcmc){
Rprintf("Batch: %i of %i, %3.2f%%\n", b+1, nBatch, 100.0*(b+1)/nBatch);
Rprintf("\tparameter\tacceptance\ttuning\n");
for(j = 0, i = 0; j < m; j++){
for(k = j; k < m; k++, i++){
Rprintf("\tA[%i,%i]\t\t%3.1f\t\t%1.2f\n", j+1, k+1, 100.0*REAL(accept_r)[b*nParams+AIndx+i], exp(tuning[AIndx+i]));
}
}
if(nugget){
for(j = 0; j < m; j++){
Rprintf("\tPsi[%i,%i]\t%3.1f\t\t%1.2f\n", j+1, j+1, 100.0*REAL(accept_r)[b*nParams+PsiIndx+j], exp(tuning[PsiIndx+j]));
}
}
for(j = 0; j < m; j++){
Rprintf("\tphi[%i]\t\t%3.1f\t\t%1.2f\n", j+1, 100.0*REAL(accept_r)[b*nParams+phiIndx+j], exp(tuning[phiIndx+j]));
}
if(covModel == "matern"){
Rprintf("\n");
for(j = 0; j < m; j++){
Rprintf("\tnu[%i]\t\t%3.1f\t\t%1.2f\n", j+1, 100.0*REAL(accept_r)[b*nParams+nuIndx+j], exp(tuning[nuIndx+j]));
}
}
}else{
Rprintf("Sampled: %i of %i, %3.2f%%\n", s, nSamples, 100.0*s/nSamples);
Rprintf("Report interval Metrop. Acceptance rate: %3.2f%%\n", 100.0*batchAccept/nReport);
Rprintf("Overall Metrop. Acceptance rate: %3.2f%%\n", 100.0*accept[0]/s);
}
Rprintf("-------------------------------------------------\n");
#ifdef Win32
R_FlushConsole();
#endif
}
if(!amcmc){
REAL(accept_r)[reportCnt] = 100.0*batchAccept/nReport;
reportCnt++;
}
status = 0;
batchAccept = 0;
}
status++;
}//end sample loop
PutRNGstate();
//untransform variance variables
for(s = 0; s < nSamples; s++){
covTransInv(&REAL(samples_r)[s*nParams+AIndx], &REAL(samples_r)[s*nParams+AIndx], m);
if(nugget){
for(i = 0; i < m; i++){
REAL(samples_r)[s*nParams+PsiIndx+i] = exp(REAL(samples_r)[s*nParams+PsiIndx+i]);
}
}
for(i = 0; i < m; i++){
REAL(samples_r)[s*nParams+phiIndx+i] = logitInv(REAL(samples_r)[s*nParams+phiIndx+i], phiUnif[i*2], phiUnif[i*2+1]);
if(covModel == "matern"){
REAL(samples_r)[s*nParams+nuIndx+i] = logitInv(REAL(samples_r)[s*nParams+nuIndx+i], nuUnif[i*2], nuUnif[i*2+1]);
}
}
}
//make return object
SEXP result_r, resultName_r;
int nResultListObjs = 2;
if(amcmc){
nResultListObjs++;
}
PROTECT(result_r = allocVector(VECSXP, nResultListObjs)); nProtect++;
PROTECT(resultName_r = allocVector(VECSXP, nResultListObjs)); nProtect++;
//samples
SET_VECTOR_ELT(result_r, 0, samples_r);
SET_VECTOR_ELT(resultName_r, 0, mkChar("p.theta.samples"));
SET_VECTOR_ELT(result_r, 1, accept_r);
SET_VECTOR_ELT(resultName_r, 1, mkChar("acceptance"));
if(amcmc){
SET_VECTOR_ELT(result_r, 2, tuning_r);
SET_VECTOR_ELT(resultName_r, 2, mkChar("tuning"));
}
namesgets(result_r, resultName_r);
//unprotect
UNPROTECT(nProtect);
return(result_r);
}
void testComplexArith(const FHEPubKey& publicKey,
const FHESecKey& secretKey,
const EncryptedArrayCx& ea, double epsilon)
{
// Test complex conjugate
Ctxt c1(publicKey), c2(publicKey);
vector<cx_double> vd;
vector<cx_double> vd1, vd2;
ea.random(vd1);
ea.random(vd2);
ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);
if (verbose)
cout << "Test Conjugate: ";
for_each(vd1.begin(), vd1.end(), [](cx_double& d){d=std::conj(d);});
c1.complexConj();
ea.decrypt(c1, secretKey, vd);
#ifdef DEBUG_PRINTOUT
printVec(cout<<"vd1=", vd1, 10)<<endl;
printVec(cout<<"res=", vd, 10)<<endl;
#endif
if (cx_equals(vd, vd1, conv<double>(epsilon*c1.getPtxtMag())))
cout << "GOOD\n";
else {
cout << "BAD:\n";
std::cout << " max(vd)="<<largestCoeff(vd)
<< ", max(vd1)="<<largestCoeff(vd1)
<< ", maxDiff="<<calcMaxDiff(vd,vd1) << endl<<endl;
}
// Test that real and imaginary parts are actually extracted.
Ctxt realCtxt(c2), imCtxt(c2);
vector<cx_double> realParts(vd2), real_dec;
vector<cx_double> imParts(vd2), im_dec;
if (verbose)
cout << "Test Real and Im parts: ";
for_each(realParts.begin(), realParts.end(), [](cx_double& d){d=std::real(d);});
for_each(imParts.begin(), imParts.end(), [](cx_double& d){d=std::imag(d);});
ea.extractRealPart(realCtxt);
ea.decrypt(realCtxt, secretKey, real_dec);
ea.extractImPart(imCtxt);
ea.decrypt(imCtxt, secretKey, im_dec);
#ifdef DEBUG_PRINTOUT
printVec(cout<<"vd2=", vd2, 10)<<endl;
printVec(cout<<"real=", realParts, 10)<<endl;
printVec(cout<<"res=", real_dec, 10)<<endl;
printVec(cout<<"im=", imParts, 10)<<endl;
printVec(cout<<"res=", im_dec, 10)<<endl;
#endif
if (cx_equals(realParts,real_dec,conv<double>(epsilon*realCtxt.getPtxtMag()))
&& cx_equals(imParts, im_dec, conv<double>(epsilon*imCtxt.getPtxtMag())))
cout << "GOOD\n";
else {
cout << "BAD:\n";
std::cout << " max(re)="<<largestCoeff(realParts)
<< ", max(re1)="<<largestCoeff(real_dec)
<< ", maxDiff="<<calcMaxDiff(realParts,real_dec) << endl;
std::cout << " max(im)="<<largestCoeff(imParts)
<< ", max(im1)="<<largestCoeff(im_dec)
<< ", maxDiff="<<calcMaxDiff(imParts,im_dec) << endl<<endl;
}
}
void testBasicArith(const FHEPubKey& publicKey,
const FHESecKey& secretKey,
const EncryptedArrayCx& ea, double epsilon)
{
if (verbose) cout << "Test Arithmetic ";
// Test objects
Ctxt c1(publicKey), c2(publicKey), c3(publicKey);
vector<cx_double> vd;
vector<cx_double> vd1, vd2, vd3;
ea.random(vd1);
ea.random(vd2);
// test encoding of shorter vectors
vd1.resize(vd1.size()-2);
ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
vd1.resize(vd1.size()+2, 0.0);
ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);
// Test - Multiplication
c1 *= c2;
for (long i=0; i<lsize(vd1); i++) vd1[i] *= vd2[i];
ZZX poly;
ea.random(vd3);
ea.encode(poly, vd3, /*size=*/1.0);
c1.addConstant(poly); // vd1*vd2 + vd3
for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i];
// Test encoding, encryption of a single number
double xx = NTL::RandomLen_long(16)/double(1L<<16); // random in [0,1]
ea.encryptOneNum(c2, publicKey, xx);
c1 += c2;
for (auto& x : vd1) x += xx;
// Test - Multiply by a mask
vector<long> mask(lsize(vd1), 1);
for (long i=0; i*(i+1)<lsize(mask); i++) {
mask[i*i] = 0;
mask[i*(i+1)] = -1;
}
ea.encode(poly,mask, /*size=*/1.0);
c1.multByConstant(poly); // mask*(vd1*vd2 + vd3)
for (long i=0; i<lsize(vd1); i++) vd1[i] *= mask[i];
// Test - Addition
ea.random(vd3);
ea.encrypt(c3, publicKey, vd3, /*size=*/1.0);
c1 += c3;
for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i];
c1.negate();
c1.addConstant(to_ZZ(1));
for (long i=0; i<lsize(vd1); i++) vd1[i] = 1.0 - vd1[i];
// Diff between approxNums HE scheme and plaintext floating
ea.decrypt(c1, secretKey, vd);
#ifdef DEBUG_PRINTOUT
printVec(cout<<"res=", vd, 10)<<endl;
printVec(cout<<"vec=", vd1, 10)<<endl;
#endif
if (verbose)
cout << "(max |res-vec|_{infty}="<< calcMaxDiff(vd, vd1) << "): ";
if (cx_equals(vd, vd1, conv<double>(epsilon*c1.getPtxtMag())))
cout << "GOOD\n";
else {
cout << "BAD:\n";
std::cout << " max(vd)="<<largestCoeff(vd)
<< ", max(vd1)="<<largestCoeff(vd1)
<< ", maxDiff="<<calcMaxDiff(vd,vd1) << endl<<endl;
}
}
// The MAIN function, from here we start the application and run the
// game loop
int main()
{
std::cout << "Starting GLFW context, OpenGL 3.3" << std::endl;
// Init GLFW
glfwInit();
// Set all the required options for GLFW
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
// OS X requests 3.3 differently.
#ifdef __APPLEOS__
glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
#endif
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);
// Create a GLFWwindow object that we can use for GLFW's functions
GLFWwindow* window = glfwCreateWindow(WIDTH, HEIGHT, "LearnOpenGL",
nullptr, nullptr);
if (window == nullptr)
{
std::cout << "Failed to create GLFW window" << std::endl;
glfwTerminate();
return -1;
}
glfwMakeContextCurrent(window);
// Set the required callback functions
glfwSetKeyCallback(window, key_callback);
// Set this to true so GLEW knows to use a modern approach to retrieving
// function pointers and extensions
// More hacking for my autocomplete.
#ifndef CLANG_COMPLETE_ONLY
glewExperimental = GL_TRUE;
// Initialize GLEW to setup the OpenGL Function pointers
if (glewInit() != GLEW_OK)
{
std::cout << "Failed to initialize GLEW" << std::endl;
return -1;
}
#endif
// Define the viewport dimensions
// Corrected from the tutorial, should request framebuffer size instead.
// Technically should also write a callback when the window is resized
// with framebuffer_size_callback. See GLFW docs. This will do for now.
int fbwidth, fbheight;
glfwGetFramebufferSize(window, &fbwidth, &fbheight);
// Options
glViewport(0, 0, WIDTH, HEIGHT);
glEnable(GL_DEPTH_TEST);
// ---------- BEGIN OPENGL ----------- //
// Shader creation
Shader edgeShader("vertex_shader_unsliced.glsl", "fragment_shader_unsliced.glsl");
// Data
GLfloat vertices[] = {
// Positions
0.6f, 0.6f, 0.6f, 0.6f,
0.6f, -0.6f, 0.6f, 0.6f,
-0.6f, -0.6f, 0.6f, 0.6f,
-0.6f, 0.6f, 0.6f, 0.6f,
0.6f, 0.6f, -0.6f, 0.6f,
0.6f, -0.6f, -0.6f, 0.6f,
-0.6f, -0.6f, -0.6f, 0.6f,
-0.6f, 0.6f, -0.6f, 0.6f,
0.8f, 0.8f, 0.8f, -0.8f,
0.8f, -0.8f, 0.8f, -0.8f,
-0.8f, -0.8f, 0.8f, -0.8f,
-0.8f, 0.8f, 0.8f, -0.8f,
0.8f, 0.8f, -0.8f, -0.8f,
0.8f, -0.8f, -0.8f, -0.8f,
-0.8f, -0.8f, -0.8f, -0.8f,
-0.8f, 0.8f, -0.8f, -0.8f,
};
GLuint indices[] = {
// Near Cube
0, 2, 1,
0, 3, 2,
7, 5, 6,
7, 4, 5,
3, 4, 7,
3, 0, 4,
6, 1, 2,
6, 5, 1,
6, 3, 7,
6, 2, 3,
1, 4, 0,
1, 5, 4,
// Far Cube
8, 10, 9,
8, 11, 10,
15, 13, 14,
15, 12, 13,
11, 12, 15,
11, 8, 12,
14, 9, 10,
14, 13, 9,
14, 11, 15,
14, 10, 11,
9, 12, 8,
9, 13, 12,
// Front Cube
0, 2, 1,
0, 3, 2,
11, 9, 10,
11, 8, 9,
3, 0, 8,
3, 8, 11,
10, 1, 2,
10, 9, 1,
10, 3, 11,
10, 2, 3,
1, 8, 0,
1, 9, 8,
// Back Cube
5, 7, 4,
5, 6, 7,
14, 12, 15,
14, 13, 12,
6, 5, 13,
6, 13, 14,
15, 4, 7,
15, 12, 4,
15, 6, 14,
15, 7, 6,
4, 13, 5,
4, 12, 13,
// Top Cube
4, 7, 3,
4, 3, 0,
15, 8, 11,
15, 12, 8,
7, 12, 15,
7, 4, 12,
0, 11, 8,
0, 3, 11,
3, 7, 15,
3, 15, 11,
4, 0, 8,
4, 8, 12,
// Bottom Cube
1, 2, 6,
1, 6, 5,
10, 13, 14,
10, 9, 13,
2, 9, 10,
2, 1, 9,
5, 14, 13,
5, 6, 14,
6, 2, 10,
6, 10, 14,
1, 5, 13,
1, 13, 9,
// Left Cube
3, 7, 6,
3, 6, 2,
15, 11, 10,
15, 10, 14,
11, 15, 7,
11, 7, 3,
14, 2, 6,
14, 10, 2,
7, 15, 14,
7, 14, 6,
11, 3, 2,
11, 2, 10,
// Right Cube
1, 5, 4,
1, 4, 0,
13, 9, 8,
13, 8, 12,
9, 13, 5,
9, 5, 1,
12, 0, 4,
12, 8, 0,
5, 13, 12,
5, 12, 4,
9, 1, 0,
9, 0, 8,
};
// Textures
// Set up buffer stuff
GLuint VAO, VBO, EBO;
glGenVertexArrays(1, &VAO);
glGenBuffers(1, &VBO);
glGenBuffers(1, &EBO);
// Game loop
while (!glfwWindowShouldClose(window))
{
// Check if any events have been activated (key pressed, mouse moved
// etc.) and call corresponding response functions
glfwPollEvents();
// Setup stuff
glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glLineWidth(3);
// 4D-3D Transformations
glm::mat4 model4D(1.0f), view4D;
GLfloat theta = glm::radians((GLfloat)glfwGetTime() * 50.0f);
GLfloat cs = cos(theta), sn = sin(theta);
model4D[0][0] = cs;
model4D[0][3] = -sn;
model4D[3][0] = sn;
model4D[3][3] = cs;
glm::mat4x3 projection4D;
glm::vec4 from(0.0f, 0.0f, 0.0f, 4.0f), to(0.0f, 0.0f, 0.0f, 0.0f);
glm::vec4 up(0.0f, 1.0f, 0.0f, 0.0f), over(0.0f, 0.0f, 1.0f, 0.0f);
view4D = lookAt4D(from, to, up, over);
std::cout << "View Mat: " << std::endl;
printMat(view4D, 4, 4);
projection4D = proj4D();
GLfloat *projVert = new GLfloat[16*7];
std::cout << "--------------------------------" << std::endl;
for(int i = 0; i != 16; i++) {
// Project each vertex to the 3D space
glm::vec4 vert4(vertices[i*4], vertices[i*4+1],
vertices[i*4+2], vertices[i*4+3]);
glm::vec4 viewVert = view4D * (model4D * vert4 - from);
glm::vec3 vert3 = projection4D * view4D * (model4D * vert4 - from);
printVec(viewVert, 4);
projVert[i*7] = vert3.x;
projVert[i*7+1] = vert3.y;
projVert[i*7+2] = vert3.z;
if (i < 8) {
projVert[i*7+3] = 1.0f;
projVert[i*7+4] = 0.0f;
projVert[i*7+5] = 0.0f;
} else {
projVert[i*7+3] = 0.0f;
projVert[i*7+4] = 0.0f;
projVert[i*7+5] = 1.0f;
}
projVert[i*7+6] = (viewVert.w + 5.0f)/2.0f;
}
// 3D-2D Transformations
glm::mat4 view3D = glm::lookAt(glm::vec3(3.0f, 1.2f, 2.0f),
glm::vec3(0.0f, 0.0f, 0.0f),
glm::vec3(0.0f, 1.0f, 0.0f));
glm::mat4 proj3D = glm::perspective(glm::radians(45.0f),
(float)WIDTH/(float)HEIGHT,
0.1f, 100.0f);
// Shader Uniforms
edgeShader.Use();
GLint viewLoc = glGetUniformLocation(edgeShader.Program,
"view3D");
GLint projectionLoc = glGetUniformLocation(edgeShader.Program,
"projection3D");
glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view3D));
glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(proj3D));
// Load Vertices
glBindVertexArray(VAO);
glBindBuffer(GL_ARRAY_BUFFER, VBO);
glBufferData(GL_ARRAY_BUFFER, 16*7*sizeof(GL_FLOAT), projVert, GL_DYNAMIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(indices), indices,
GL_DYNAMIC_DRAW);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GL_FLOAT),
(GLvoid*)0);
glEnableVertexAttribArray(0);
glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 7*sizeof(GL_FLOAT),
(GLvoid*)(3*sizeof(GL_FLOAT)));
glEnableVertexAttribArray(1);
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glDrawElements(GL_TRIANGLES, 8*12*3, GL_UNSIGNED_INT, 0);
glBindVertexArray(0);
// Swap the screen buffers
glfwSwapBuffers(window);
delete[] projVert;
}
// Terminate GLFW, clearing any resources allocated by GLFW.
glfwTerminate();
glDeleteVertexArrays(1, &VAO);
glDeleteBuffers(1, &VBO);
glDeleteBuffers(1, &EBO);
return 0;
}
int main(int argc, char **argv) {
GDISPLAY *video, *feedback;
GPOINT *gPoints;
XEvent event;
WSETUP *wayv;
WGESTURE *gaction;
MATRIX *gesture;
VECTOR *vector;
int gPosition = 0;
char *file;
printf("wayV version %s\n", VERSION);
/* Decide what configuration file to open */
if(argc != 2)
file = strdup(SETUPFILE);
else
file = strdup(argv[1]);
if((wayv = readSetup(file)) == NULL)
exit(BAD);
// writeSetup(wayv, SETUPOUT);
/* Setup the display */
if(!(video = setupDrawable(wayv->pretty, wayv->pretty->window, argc, argv))) {
printf("Cannot connected to X server\n");
return BAD;
}
/* Setup the mouse/pointer as an input device */
setupPointer(wayv, video);
/* Allocate memory to store the gesture points */
gPoints = setupGPoints(wayv->universe->maxgpoints);
if(wayv->pretty->display[0] == 'i')
handleDrawableWindow(video, MAP);
setupSignals();
/* Process the events */
while(1) {
XNextEvent(video->display, &event);
switch(event.type) {
case MotionNotify:
if(gPosition < wayv->universe->maxgpoints) {
gPoints[gPosition].x = event.xmotion.x;
gPoints[gPosition].y = event.xmotion.y;
if(wayv->pretty->display[0] == 'y')
writePixels(video, (int)gPoints[gPosition-1].x - GDEF_BORDER,
(int)gPoints[gPosition-1].y - GDEF_BORDER,
(int)gPoints[gPosition].x - GDEF_BORDER,
(int)gPoints[gPosition].y - GDEF_BORDER);
else if(wayv->pretty->display[0] == 'i')
writePixels(video, (int)gPoints[gPosition-1].x,
(int)gPoints[gPosition-1].y,
(int)gPoints[gPosition].x,
(int)gPoints[gPosition].y);
gPosition++;
}
break;
case ButtonPress:
if(wayv->pretty->display[0] == 'y')
handleDrawableWindow(video, MAP);
gPosition = 0;
gPoints[gPosition].x = event.xbutton.x;
gPoints[gPosition].y = event.xbutton.y;
gPosition++;
break;
case ButtonRelease:
if(wayv->pretty->display[0] == 'y')
handleDrawableWindow(video, UNMAP);
gPoints[gPosition].x = GEND;
gPoints[gPosition].y = GEND;
gesture = gridGPoints(gPoints, wayv->universe->xgrid,
wayv->universe->ygrid);
vector = vectGPoints(gPoints, wayv->universe->xgrid,
wayv->universe->ygrid, wayv->universe->maxvectors);
printf("\n\n=======Gesture=======");
printMat(gesture);
printVec(vector);
if(wayv->pretty->display[0] != 'n')
handleDrawableWindow(video, CLEAR);
if((gaction = findGesture(wayv, gesture, vector, wayv->think))) {
printf("Is Action : %s\n", gaction->name);
performAction(wayv, gaction, video, gPoints);
/* Fork off program to handle feedback window */
if(wayv->pretty->feedback[0] == 'y' && fork() == 0) {
feedback = writeText(gaction->action);
waitMilli(wayv->pretty->wait);
handleDrawableWindow(feedback, DESTROY);
close(ConnectionNumber(video->display));
setsid();
exit(BAD);
}
}
freeMat(gesture);
free(vector);
break;
case ClientMessage:
if(event.xclient.data.l[0] == video->wmdelete) {
closeDisplay(video);
exit(GOOD);
}
break;
}
}
}
