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;
}
}
