int triangleEvaluateHalfSpace(const Triangle3DSetup &tri, VecRef xShuffle, VecRef yShuffle) { Vec DX12 = vec(tri.dx1); Vec DX23 = vec(tri.dx2); Vec DX31 = vec(tri.dx3); Vec DY12 = vec(tri.dy1); Vec DY23 = vec(tri.dy2); Vec DY31 = vec(tri.dy3); TriangleSetup s; triangleCalculateConstants(tri, s); Vec C1 = vec(s.c1); Vec C2 = vec(s.c2); Vec C3 = vec(s.c3); const Vec Zero = vec(0); const Vec p = vecCmpGT(vecSub(vecMadd(DX12, yShuffle, C1), vecMul(DY12, xShuffle)), Zero); const Vec q = vecCmpGT(vecSub(vecMadd(DX23, yShuffle, C2), vecMul(DY23, xShuffle)), Zero); const Vec r = vecCmpGT(vecSub(vecMadd(DX31, yShuffle, C3), vecMul(DY31, xShuffle)), Zero); #ifndef PLATFORM_PS3 const int a = vecMaskToInt(p); const int b = vecMaskToInt(q); const int c = vecMaskToInt(r); if(!a || !b || !c) { return FullyOutside; } else if(a == 0xf && b == 0xf && c == 0xf) { return FullyCovered; } else { return PartiallyInside; } #else vec_uint4 ones = (vec_uint4){0xffffffff,0xffffffff,0xffffffff,0xffffffff}; vec_uint4 zeros = (vec_uint4){0, 0, 0, 0}; if(vec_all_eq((vec_uint4)p, zeros) || vec_all_eq((vec_uint4)q, zeros) || vec_all_eq((vec_uint4)r, zeros)) { return FullyOutside; } else if(vec_all_eq((vec_uint4)p, ones) && vec_all_eq((vec_uint4)q, ones) && vec_all_eq((vec_uint4)r, ones)) { return FullyCovered; } else { return PartiallyInside; } #endif }
static void triangleDrawTileFast(float *tilebuffer, const Triangle3DSetup &t, int x, int y) { const Vec denom = vecRcp(vec(t.dx1 * t.dy2 - t.dy1 * t.dx2)); const Vec DY2 = vec(t.dy2); const Vec DY3 = vec(t.dy3); const Vec DX2 = vec(t.dx2); const Vec DX3 = vec(t.dx3); const Vec X3 = vec(t.x3); const Vec Y3 = vec(t.y3); const Vec Z1 = vec(t.z1); const Vec Z2 = vec(t.z2); const Vec Z3 = vec(t.z3); const Vec One = vec(1); const Vec Step = vec(0, 1, 2, 3); for(int iy = 0; iy < tileHeight; iy++) { Vec yp = vec(float(y + iy)); float fx = (float)x; const Vec ypos = vecSub(yp, Y3); const Vec dxypos2 = vecMul(DX2, ypos); const Vec dxypos3 = vecMul(DX3, ypos); for(int ix = 0; ix < tileWidth; ix += 4, fx += 4) { void * ptr = &tilebuffer[ix + iy * tileWidth]; const Vec xp = vecAdd(vec(fx), Step); // Calculate barycentric coordinates and depth const Vec xpos = vecSub(xp, X3); const Vec dyxpos2 = vecMul(DY2, xpos); const Vec dyxpos3 = vecMul(DY3, xpos); const Vec delta2 = vecSub(dyxpos2, dxypos2); const Vec delta3 = vecSub(dyxpos3, dxypos3); const Vec t1 = vecMul(delta2, denom); const Vec t2 = vecMul(delta3, denom); const Vec t3 = vecSub(vecSub(One, t1), t2); const Vec tz1 = vecMul(Z1, t1); const Vec tz2 = vecMul(Z2, t2); const Vec tz3 = vecMul(Z3, t3); const Vec z = vecAdd(tz1, vecAdd(tz2, tz3)); // Depth compare const Vec Value = vecLoadAligned(ptr); const Vec DepthCompare = vecCmpGE(Value, z); const Vec Result = vecSel(Value, z, DepthCompare); vecStoreAligned(ptr, Result); } } }
void GLColor(const float r, const float g, const float b) { // when the light is enable Vec3f v,V; vecInit( V, r,g,b ); vecMul( v, 0.05f, V); glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, ((float*)(&v)) ); vecMul( v, 1.f, V); glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, ((float*)(&v)) ); vecMul( v, 0.1f, V); glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, ((float*)(&v)) ); // when the light is disable glColor3f( r,g,b); }
/* double calcRange(GEOLOCATE_REC *g, double look, double yaw); Calculates the slant range, at the given look and yaw angle, from the given satellite's position to the earth's surface. */ double calcRange(GEOLOCATE_REC *g,double look,double yaw) { vector rvec; vector sarPos=g->stVec.pos; double ans1,ans2; double re2,rp2; double a,b,c,d; rvec.x= sin(yaw); rvec.y=-sin(look)*cos(yaw); rvec.z=-cos(look)*cos(yaw); /* Unit vector rvec points to target. Rotate into earth centered vector:*/ vecMul(g->look_matrix,rvec,&rvec); /* calculate range to intercept the earth (modeled as an ellipsoid). */ re2=g->re*g->re; rp2=g->rp*g->rp; a=(rvec.x*rvec.x+rvec.y*rvec.y)/re2 + rvec.z*rvec.z/rp2; b=2.0*((sarPos.x*rvec.x + sarPos.y*rvec.y)/re2 + sarPos.z*rvec.z/rp2); c=(sarPos.x*sarPos.x+sarPos.y*sarPos.y)/re2 + sarPos.z*sarPos.z/rp2 - 1.0; /* quadradic formula...save nearer range point (the first Earth intersection).*/ d=(b*b-4.0*a*c); if (d<0) return -1.0; /* Path does not intersect earth. */ ans1=(-b + sqrt(d))/(2.0*a); ans2=(-b - sqrt(d))/(2.0*a); if (ans1<ans2) return ans1; else return ans2; }
int triangleClipToPlane(Vec *src, Vec *dst, int size, Vec plane) { int idx = 0; Vec current = *src; for(int i = 1; i < size + 1; i++) { Vec next = src[i % size]; Vec distNext = vecDot(plane, next); Vec distCurrent = vecDot(plane, current); Vec delta = vecSub(distCurrent, distNext); bool goingInNext = vecGetElem(distNext, 0) > 0; bool crossingEdge = goingInNext != vecGetElem(distCurrent, 0) > 0; if(crossingEdge) { // output interpolated vertex Vec ratio = vecMul(distCurrent, vecRcp(delta)); dst[idx] = vecAdd(current, vecMul(vecSub(next, current), ratio)); idx++; } if(goingInNext) { // output vertex as-is dst[idx] = next; idx++; } current = next; } return idx; }
void PhysicsWorld::ResolveCollisions() { for(unsigned int i = 0; i < collidedObjectsCnt; i++) { CollisionInfo &inf = collidedObjects[i]; // Just push the contact out by the velocity so we never // end up in a state where the two objects are still intersecting Transform *t = entGetTransform(inf.obj->ent); inf.obj->position = t->GetPosition(); inf.obj->position = vecSub(inf.obj->position, inf.obj->velocity); inf.obj->velocity = vecReflect(inf.obj->velocity, inf.normal); inf.obj->velocity = vecMul(inf.obj->velocity, vec(1, 1, 0, 0)); inf.obj->position = vecAdd(inf.obj->position, inf.obj->velocity); t->SetPosition(inf.obj->position); } collidedObjectsCnt = 0; }
static void get_target_position(stateVector *st, double look, double yaw, double sr, vector *targPos) { vector relPos = vecNew(sin(yaw), -sin(look)*cos(yaw), -cos(look)*cos(yaw)); // relPos unit vector points from s/c to targPos. // Rotate into earth axes vector look_matrix[3]; look_matrix[2] = st->pos; vecNormalize(&look_matrix[2]); vector v = st->vel; vecNormalize(&v); vecCross(look_matrix[2],v,&look_matrix[1]); vecCross(look_matrix[1],look_matrix[2],&look_matrix[0]); vecMul(look_matrix,relPos,&relPos); // scale relPos so it reaches from s/c to targPos vecScale(&relPos,sr); vecAdd(relPos,st->pos,targPos); }
void triangleVertexShading(JmJob *data) { VertexJob v; dmaBlockGet(&v, (uintptr_t)data->p1, sizeof(VertexJob), DmaTag); Vec halfSizeAdd = vec(0.5f * sw, 0.5f * sh, 0, 1); Vec halfSizeMul = vec(0.5f * sw, -0.5f * sh, 1, 0); Vec zero = vec(0); Mat transform; matMul(&transform, v.projection, v.world); while(true) { unsigned int k = interlockedExchangeAdd(v.counter, VerticesAtATime); if(k >= v.end) break; unsigned int end = std::min(k + VerticesAtATime, v.end); unsigned int num = end - k; unsigned int numtris = num / 3; #ifndef PLATFORM_PS3_SPU Triangle3D vertexShadingTris[TriangleAtATime]; #endif dmaBlockGet(vertexShadingTris, (uintptr_t)&v.input[k], numtris * sizeof(Triangle3D), DmaTag); for(unsigned int u = 0; u < numtris; u++) { const Triangle3D &tri = vertexShadingTris[u]; Triangle3D triOut[5]; Vec vtx[8]; int numTrisOut = 1; if(g_EnableBackfaceCulling) { // We can't do the backface culling using a determinant of a 2x2 matrix // in screen space because then we would have 'holes' in the output data // therefor it happens here, in wordspace. Vec e1 = vecSub(tri.p3, tri.p1); Vec e2 = vecSub(tri.p2, tri.p1); Vec n = vecCross(e1, e2); Vec a = vecDot(v.camerapos, n); if(vecGetElem(a, VecComponent::X) > 0) continue; } // perspective project matMulVec(&triOut[0].p1, transform, tri.p1); matMulVec(&triOut[0].p2, transform, tri.p2); matMulVec(&triOut[0].p3, transform, tri.p3); // cull against znear Vec m1 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p1), zero); Vec m2 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p2), zero); Vec m3 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p3), zero); Vec c2 = vecAnd(vecAnd(m1, m2), m3); #ifdef PLATFORM_PS3 vec_uint4 ones = (vec_uint4){0xffffffff,0xffffffff,0xffffffff,0xffffffff}; if(vec_all_eq((vec_uint4)c2, ones)) continue; #else int result = vecMaskToInt(c2); if(result == 15) continue; // discard, all behind nearz #endif #if 1 // clip triangles that intersect znear static const int NumVerticesInATriangle = 3; int numVertsOut = triangleClipToPlane( (Vec*)&triOut[0], vtx, NumVerticesInATriangle, vec(0, 0, 1, 0) ); // Very simple triangulation routine numTrisOut = 0; for(int i = 2; i < numVertsOut; i++) { triOut[numTrisOut].p1 = vtx[0]; triOut[numTrisOut].p2 = vtx[i]; triOut[numTrisOut].p3 = vtx[i - 1]; numTrisOut++; } #endif for(int i = 0; i < numTrisOut; i++) { // perspective divide triOut[i].p1 = vecMul(triOut[i].p1, vecRcp(vecSplat<VecComponent::W>(triOut[i].p1))); triOut[i].p2 = vecMul(triOut[i].p2, vecRcp(vecSplat<VecComponent::W>(triOut[i].p2))); triOut[i].p3 = vecMul(triOut[i].p3, vecRcp(vecSplat<VecComponent::W>(triOut[i].p3))); // transform to screen space Vec r1 = vecMadd(triOut[i].p1, halfSizeMul, halfSizeAdd); Vec r2 = vecMadd(triOut[i].p2, halfSizeMul, halfSizeAdd); Vec r3 = vecMadd(triOut[i].p3, halfSizeMul, halfSizeAdd); #ifdef PLATFORM_PS3_SPU Triangle3DSetup &r = setup[s][sidx]; #else Triangle3DSetup r; #endif memcpy(&r.x1, &r1, sizeof(float) * 3); memcpy(&r.x2, &r2, sizeof(float) * 3); memcpy(&r.x3, &r3, sizeof(float) * 3); // deltas r.dx1 = r.x1 - r.x2; r.dx2 = r.x2 - r.x3; r.dx3 = r.x3 - r.x1; r.dy1 = r.y1 - r.y2; r.dy2 = r.y2 - r.y3; r.dy3 = r.y3 - r.y1; #ifdef PLATFORM_PS3_SPU sidx++; if(sidx >= MaxSetupBuffered) { dmaWaitAll(1 << DmaListTag); unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx); for(unsigned int u = 0; u < sidx; u++) { setuplist[u].notify = 0; setuplist[u].reserved = 0; setuplist[u].size = sizeof(Triangle3DSetup); setuplist[u].eal = (uintptr_t)&v.output[u + l]; } cellDmaListPut(setup[s], 0, setuplist, sizeof(setuplist), DmaListTag, 0, 0); sidx = 0; s ^= 1; } #else unsigned int l = interlockedExchangeAdd(v.outputCnt, 1); if(l >= MaxTrianglesDrawn) { interlockedExchangeSub(v.outputCnt, 1); break; } else dmaBlockPut(&r, (uintptr_t)&v.output[l], sizeof(Triangle3DSetup), DmaTag); #endif } } } #ifdef PLATFORM_PS3_SPU if(sidx > 0) { dmaWaitAll(1 << DmaListTag); unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx); for(unsigned int u = 0; u < sidx; u++) { setuplist[u].notify = 0; setuplist[u].reserved = 0; setuplist[u].size = sizeof(Triangle3DSetup); setuplist[u].eal = (uintptr_t)&v.output[u + l]; } cellDmaListPut(setup[s], 0, setuplist, sidx * sizeof(CellDmaListElement), DmaListTag + 1, 0, 0); dmaWaitAll(1 << (DmaListTag + 1)); } #endif }
int main() { setvbuf(stdout, NULL, _IONBF, 0); setvbuf(stderr, NULL, _IONBF, 0); /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f30x.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f30x.c file */ /* SysTick end of count event each 10ms */ RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */ USART1_Init(); //printf("Starting\n"); USART1_flush(); /* printf("Initialising USB\n"); USBHID_Init(); printf("Initialising USB HID\n"); Joystick_init(); */ /* Initialise LEDs */ //printf("Initialising LEDs\n"); int i; for (i = 0; i < 8; ++i) { STM_EVAL_LEDInit(leds[i]); STM_EVAL_LEDOff(leds[i]); } /* Initialise gyro */ //printf("Initialising gyroscope\n"); Gyro_Init(); /* Initialise compass */ //printf("Initialising compass\n"); Compass_Init(); Delay(100); calibrate(); int C = 0, noAccelCount = 0; while (1) { float *acc = accs[C&1], *prevAcc = accs[(C&1)^1], *vel = vels[C&1], *prevVel = vels[(C&1)^1], *pos = poss[C&1], *prevPos = poss[(C&1)^1], *angRate = angRates[C&1], *prevAngRate = angRates[(C&1)^1], *ang = angs[C&1], *prevAng = angs[(C&1)^1], *mag = mags[C&1], *prevmag = mags[(C&1)^1]; /* Wait for data ready */ #if 0 Compass_ReadAccAvg(acc, 10); vecMul(axes, acc); //printf("X: %9.3f Y: %9.3f Z: %9.3f\n", acc[0], acc[1], acc[2]); float grav = acc[2]; acc[2] = 0; if (noAccelCount++ > 50) { for (i = 0; i < 2; ++i) { vel[i] = 0; prevVel[i] = 0; } noAccelCount = 0; } if (vecLen(acc) > 50.f) { for (i = 0; i < 2; ++i) { vel[i] += prevAcc[i] + (acc[i]-prevAcc[i])/2.f; pos[i] += prevVel[i] + (vel[i]-prevVel[i])/2.f; } noAccelCount = 0; } C += 1; if (((C) & 0x7F) == 0) { printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n", vel[0], vel[1], pos[0], pos[1], grav); //printf("%3.1f%% %d %5.1f %6.3f\n", (float) timeReadI2C*100.f / totalTime, C, (float) C*100.f / (totalTime), grav); } #endif Compass_ReadMagAvg(mag, 2); vecMul(axes, mag); float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI; if (compassAngle > 180.f) compassAngle -= 360.f; //vecNorm(mag); Gyro_ReadAngRateAvg(mag, 2); printf("%6.3f:%6.3f,%6.3f,%6.3f\n", compassAngle, mag[0], mag[1], mag[2]); #if 0 Gyro_ReadAngRateAvg(angRate, 2); angRate[0] *= 180.f / PI; angRate[1] *= 180.f / PI; angRate[2] *= 180.f / PI; float s[3] = {sin(angRate[0]), sin(angRate[1]), sin(angRate[2])}; float c[3] = {cos(angRate[0]), cos(angRate[1]), cos(angRate[2])}; float gyroMat[3][3] = { {c[0]*c[1], c[0]*s[1], -s[1]}, {c[0]*s[1]*s[2]-s[0]*c[2], c[0]*c[2]+s[0]*s[1]*s[2], c[1]*s[2]}, {c[0]*s[1]*c[2]+s[0]*s[2], -c[0]*s[2]+s[0]*s[1]*c[2], c[1]*c[2]}}; /* float gyroWorldMat[3][3]; vecMulMatTrans(gyroWorldMat, axes, gyroMat); *ang = gyroWorldMat[2][0]; *ang += gyroWorldMat[2][1]; *ang += gyroWorldMat[2][2]; *ang /= 300.f; static const float ANGALPHA = 0.0f; *ang += ANGALPHA*(compassAngle - *ang); */ float rotObsVec[3]; memcpy(rotObsVec, axes[0], sizeof(rotObsVec)); vecMul(gyroMat, rotObsVec); vecMul(axes, rotObsVec); rotObsVec[2] = 0.f; vecNorm(rotObsVec); float angDelta = acos(rotObsVec[0]); if (((++C) & 0x7) == 0) { printf("%6.3f %6.3f %6.3f %6.3f\n", rotObsVec[0], rotObsVec[1], rotObsVec[2], angDelta); } #endif #if 0 float angRate[3]; /* Read Gyro Angular data */ Gyro_ReadAngRate(angRate); printf("X: %f Y: %f Z: %f\n", angRate[0], angRate[1], angRate[2]); float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f}; float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f; float fTiltedX,fTiltedY = 0.0f; Compass_ReadMag(MagBuffer); Compass_ReadAcc(AccBuffer); for(i=0;i<3;i++) AccBuffer[i] /= 100.0f; fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2])); fSinRoll = -AccBuffer[1]/fNormAcc; fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll)); fSinPitch = AccBuffer[0]/fNormAcc; fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch)); if ( fSinRoll >0) { if (fCosRoll>0) { RollAng = acos(fCosRoll)*180/PI; } else { RollAng = acos(fCosRoll)*180/PI + 180; } } else { if (fCosRoll>0) { RollAng = acos(fCosRoll)*180/PI + 360; } else { RollAng = acos(fCosRoll)*180/PI + 180; } } if ( fSinPitch >0) { if (fCosPitch>0) { PitchAng = acos(fCosPitch)*180/PI; } else { PitchAng = acos(fCosPitch)*180/PI + 180; } } else { if (fCosPitch>0) { PitchAng = acos(fCosPitch)*180/PI + 360; } else { PitchAng = acos(fCosPitch)*180/PI + 180; } } if (RollAng >=360) { RollAng = RollAng - 360; } if (PitchAng >=360) { PitchAng = PitchAng - 360; } fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch; fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch; HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI; printf("Compass heading: %f\n", HeadingValue); #endif } return 1; }
int getDoppler(GEOLOCATE_REC *g,double look,double yaw, double *fd, double *fdot, vector *out_targPos,vector *out_relVel) { vector relPos, /*Vector from spacecraft to targPos.*/ sarPos, /*Position of spacecraft*/ targPos, /*Position of target point.*/ relVel, /*Relative velocity vector.*/ sarVel, /*Velocity of spacecraft*/ targVel, /*Velocity of targPos*/ sarAcc, /*Accelleration of spacecraft*/ targAcc, /*Accelleration of targPos*/ relAcc; /*Relative sarAccelleration*/ double range,angVel=g->angularVelocity; sarPos=g->stVec.pos; sarVel=g->stVec.vel; relPos.x= sin(yaw); relPos.y=-sin(look)*cos(yaw); relPos.z=-cos(look)*cos(yaw); /*c relPos unit vector points from s/c to targPos. Rotate into earth axes:*/ vecMul(g->look_matrix,relPos,&relPos); /*c scale relPos so it reaches from s/c to targPos */ range = calcRange(g,look,yaw); vecScale(&relPos,range); vecAdd(relPos,sarPos,&targPos); /*c c c Now we have all three vectors in earth centered coordinates: c sarPos = sar satellite position c relPos = range vector from sar to targPos c targPos = target position c c calculate velocity vectors targVel and relVel. c*/ targVel.x= -angVel*targPos.y; targVel.y= angVel*targPos.x; targVel.z= 0.0; vecSub(targVel,sarVel,&relVel); /*c c Calcuate accelerations of sar and targPos sarAcc,targAcc c */ sarAcc.x=0.0; sarAcc.y=0.0; /*c sar sarAcceleration toward earth center, via orbital considerations (accelleration is straight down, at -GxMe / h**2) */ sarAcc.z=-g->gxMe/vecDot(sarPos,sarPos); vecMul(g->look_matrix,sarAcc,&sarAcc);/* !put in e.c. coordinates c calculate sarAcceleration of targPos on earth surface:*/ targAcc.x=-targPos.x*angVel*angVel; targAcc.y=-targPos.y*angVel*angVel; targAcc.z=0.0; vecSub(targAcc,sarAcc,&relAcc); /*c c calculate doppler parameters c*/ if (out_targPos) *out_targPos=targPos; if (out_relVel) *out_relVel=relVel; if (fd) *fd=-2.0/(g->lambda*range)*vecDot(relPos,relVel); if (fdot) *fdot=-2.0/(g->lambda*range)*(vecDot(relVel,relVel)+vecDot(relPos,relAcc)); /* success */ return 0; }
int main() { setvbuf(stdout, NULL, _IONBF, 0); setvbuf(stderr, NULL, _IONBF, 0); /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f30x.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f30x.c file */ /* SysTick end of count event each 10ms */ RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */ USART1_Init(); //printf("Starting\n"); USART1_flush(); /* Initialise LEDs */ //printf("Initialising LEDs\n"); int i; for (i = 0; i < 8; ++i) { STM_EVAL_LEDInit(leds[i]); STM_EVAL_LEDOff(leds[i]); } /* Initialise gyro */ //printf("Initialising gyroscope\n"); Gyro_Init(); /* Initialise compass */ //printf("Initialising compass\n"); Compass_Init(); Delay(100); // perform calibration calibrate(); while (1) { float angRate[3], mag[3]; // read average compass values Compass_ReadMagAvg(mag, 2); // rotate the compass values so that they are aligned with Earth vecMul(axes, mag); // calculate the heading through inverse tan of the Y/X magnetic strength float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI; // fix heading to be in range -180 to 180 if (compassAngle > 180.f) compassAngle -= 360.f; // read average gyro values Gyro_ReadAngRateAvg(angRate, 2); // print out everything printf("c%6.3f\ng%6.3f\n", compassAngle, angRate[2]-zeroAngRate[2]); } return 1; }
bool PhysicsWorld::Intersects(const StaticPhysicsEntity &s, const DynamicPhysicsEntity &d, Vec &outNormal, Vec &outDir) { float dmin = 0, radiusSquared; Vec min, max, center, scale; min = vec(-1, -1, -1, 0); max = vec(1, 1, 1, 0); scale = vec(1, 1, 1, 0); Mat m, sc; center = entGetTransform(d.ent)->GetPosition(); entGetTransform(s.ent)->GetMatrix(&m); entGetTransform(d.ent)->GetMatrix(&sc); // scale the unit vector and take the largest component // as the radius for the sphere; as long as we don't do // silly rotations we should be ok matMulVec(&scale, sc, scale); radiusSquared = vecMax(scale); radiusSquared *= radiusSquared; // we don't have an inv matrix of anything so instead // we transform everything to worldspace and do the // calculations there // rot + scale matMulVec(&min, m, min); matMulVec(&max, m, max); // pos Vec p = entGetTransform(s.ent)->GetPosition(); min = vecAdd(min, p); max = vecAdd(max, p); // the actual intersection test (don't test w) for(int i = 0; i < 3; i++) { float C = vecGetElem(center, i); float Bmin = vecGetElem(min, i); float Bmax = vecGetElem(max, i); if(C < Bmin) dmin += square(C - Bmin); else if(C > Bmax) dmin += square(C - Bmax); } if(dmin <= radiusSquared) { Ray r; r.o = center; r.d = vecSub(p, center); r.d = vecNormalize(r.d); float dist; bool hit = RayBoxIntersection(r, min, max, dist, outNormal); outDir = vecMul(r.d, vec(dist)); return hit; } return false; }
static void triangleDrawTile(float *tilebuffer, const Triangle3DSetup &t, const TriangleSetup &s, int x, int y) { const Vec denom = vecRcp(vec(t.dx1 * t.dy2 - t.dy1 * t.dx2)); const Vec DY1 = vec(t.dy1); const Vec DY2 = vec(t.dy2); const Vec DY3 = vec(t.dy3); const Vec DX2 = vec(t.dx2); const Vec DX3 = vec(t.dx3); const Vec X3 = vec(t.x3); const Vec Y3 = vec(t.y3); const Vec Z1 = vec(t.z1); const Vec Z2 = vec(t.z2); const Vec Z3 = vec(t.z3); const Vec Step = vec(0, 1, 2, 3); const Vec Zero = vec(0); const Vec One = vec(1); const Vec Four = vec(4); float c1 = s.c1; float c2 = s.c2; float c3 = s.c3; for(int iy = 0; iy < tileHeight; iy++) { Vec v = vec(0, 1, 2, 3); Vec C1 = vec(c1); Vec C2 = vec(c2); Vec C3 = vec(c3); const Vec yp = vec((float)(iy + y)); const Vec ypos = vecSub(yp, Y3); const Vec dxypos2 = vecMul(DX2, ypos); const Vec dxypos3 = vecMul(DX3, ypos); float fx = (float)x; for(int ix = 0; ix < tileWidth; ix += 4) { void * ptr = &tilebuffer[ix + iy * tileWidth]; const Vec xp = vecAdd(vec(fx), Step); // Evaluate half-space equation & generate write-mask const Vec CX1 = vecSub(C1, vecMul(v, DY1)); const Vec CX2 = vecSub(C2, vecMul(v, DY2)); const Vec CX3 = vecSub(C3, vecMul(v, DY3)); const Vec Mask1 = vecCmpGT(CX1, Zero); const Vec Mask2 = vecCmpGT(CX2, Zero); const Vec Mask3 = vecCmpGT(CX3, Zero); const Vec WriteMask = vecAnd(Mask1, vecAnd(Mask2, Mask3)); // Calculate depth based on barycentric coordinates const Vec xpos = vecSub(xp, X3); const Vec dyxpos2 = vecMul(DY2, xpos); const Vec dyxpos3 = vecMul(DY3, xpos); const Vec delta2 = vecSub(dyxpos2, dxypos2); const Vec delta3 = vecSub(dyxpos3, dxypos3); const Vec t1 = vecMul(delta2, denom); const Vec t2 = vecMul(delta3, denom); const Vec t3 = vecSub(vecSub(One, t1), t2); const Vec tz1 = vecMul(Z1, t1); const Vec tz2 = vecMul(Z2, t2); const Vec tz3 = vecMul(Z3, t3); const Vec z = vecAdd(tz1, vecAdd(tz2, tz3)); // Depth compare const Vec Value = vecLoadAligned(ptr); const Vec DepthCompare = vecCmpGE(Value, z); const Vec Compare = vecAnd(WriteMask, DepthCompare); const Vec Result = vecSel(Value, z, Compare); vecStoreAligned(ptr, Result); v = vecAdd(v, Four); fx += 4; } c1 += t.dx1; c2 += t.dx2; c3 += t.dx3; } }