// Wait for a change in clock level and measure the time it took.
static inline unsigned long waitCLKchange(struct timespec *tp, int currentState)
{
unsigned long c = 0;
while (GET_GPIO(PI_CLOCK_IN) == currentState) {
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, tp, NULL);
tp->tv_nsec += INTERVAL;
tnorm(tp);
c += INTERVAL;
//printf("cs:%i,c:%lu,t.tv_sec:%lu,t.tv_nsec:%lu\n",currentState,c,tp->tv_sec,tp->tv_nsec);
}
return c; // time between change in nanoseconds
} // waitCLKchange
//------------------------------------------------------------------------------
double RNG::rtinvchi2(double scale, double trunc)
{
double R = trunc / scale;
// double X = 0.0;
// // I need to consider using a different truncated normal sampler.
// double E1 = r.expon_rate(1.0); double E2 = r.expon_rate(1.0);
// while ( (E1*E1) > (2 * E2 / R)) {
// // printf("E %g %g %g %g\n", E1, E2, E1*E1, 2*E2/R);
// E1 = r.expon_rate(1.0); E2 = r.expon_rate(1.0);
// }
// // printf("E %g %g \n", E1, E2);
// X = 1 + E1 * R;
// X = R / (X * X);
// X = scale * X;
double E = tnorm(1/sqrt(R));
double X = scale / (E*E);
return X;
}
void TransformDrawEngine::SoftwareTransformAndDraw(
int prim, u8 *decoded, LinkedShader *program, int vertexCount, u32 vertType, void *inds, int indexType, const DecVtxFormat &decVtxFormat, int maxIndex) {
bool throughmode = (vertType & GE_VTYPE_THROUGH_MASK) != 0;
bool lmode = gstate.isUsingSecondaryColor() && gstate.isLightingEnabled();
// TODO: Split up into multiple draw calls for GLES 2.0 where you can't guarantee support for more than 0x10000 verts.
#if defined(MOBILE_DEVICE)
if (vertexCount > 0x10000/3)
vertexCount = 0x10000/3;
#endif
float uscale = 1.0f;
float vscale = 1.0f;
bool scaleUV = false;
if (throughmode) {
uscale /= gstate_c.curTextureWidth;
vscale /= gstate_c.curTextureHeight;
} else {
scaleUV = !g_Config.bPrescaleUV;
}
bool skinningEnabled = vertTypeIsSkinningEnabled(vertType);
int w = gstate.getTextureWidth(0);
int h = gstate.getTextureHeight(0);
float widthFactor = (float) w / (float) gstate_c.curTextureWidth;
float heightFactor = (float) h / (float) gstate_c.curTextureHeight;
Lighter lighter(vertType);
float fog_end = getFloat24(gstate.fog1);
float fog_slope = getFloat24(gstate.fog2);
VertexReader reader(decoded, decVtxFormat, vertType);
for (int index = 0; index < maxIndex; index++) {
reader.Goto(index);
float v[3] = {0, 0, 0};
float c0[4] = {1, 1, 1, 1};
float c1[4] = {0, 0, 0, 0};
float uv[3] = {0, 0, 1};
float fogCoef = 1.0f;
if (throughmode) {
// Do not touch the coordinates or the colors. No lighting.
reader.ReadPos(v);
if (reader.hasColor0()) {
reader.ReadColor0(c0);
for (int j = 0; j < 4; j++) {
c1[j] = 0.0f;
}
} else {
c0[0] = gstate.getMaterialAmbientR() / 255.f;
c0[1] = gstate.getMaterialAmbientG() / 255.f;
c0[2] = gstate.getMaterialAmbientB() / 255.f;
c0[3] = gstate.getMaterialAmbientA() / 255.f;
}
if (reader.hasUV()) {
reader.ReadUV(uv);
uv[0] *= uscale;
uv[1] *= vscale;
}
fogCoef = 1.0f;
// Scale UV?
} else {
// We do software T&L for now
float out[3], norm[3];
float pos[3], nrm[3];
Vec3f normal(0, 0, 1);
reader.ReadPos(pos);
if (reader.hasNormal())
reader.ReadNrm(nrm);
if (!skinningEnabled) {
Vec3ByMatrix43(out, pos, gstate.worldMatrix);
if (reader.hasNormal()) {
Norm3ByMatrix43(norm, nrm, gstate.worldMatrix);
normal = Vec3f(norm).Normalized();
}
} else {
float weights[8];
reader.ReadWeights(weights);
// Skinning
Vec3f psum(0,0,0);
Vec3f nsum(0,0,0);
for (int i = 0; i < vertTypeGetNumBoneWeights(vertType); i++) {
if (weights[i] != 0.0f) {
Vec3ByMatrix43(out, pos, gstate.boneMatrix+i*12);
Vec3f tpos(out);
psum += tpos * weights[i];
if (reader.hasNormal()) {
Norm3ByMatrix43(norm, nrm, gstate.boneMatrix+i*12);
Vec3f tnorm(norm);
nsum += tnorm * weights[i];
}
}
}
// Yes, we really must multiply by the world matrix too.
Vec3ByMatrix43(out, psum.AsArray(), gstate.worldMatrix);
if (reader.hasNormal()) {
Norm3ByMatrix43(norm, nsum.AsArray(), gstate.worldMatrix);
normal = Vec3f(norm).Normalized();
}
}
// Perform lighting here if enabled. don't need to check through, it's checked above.
float unlitColor[4] = {1, 1, 1, 1};
if (reader.hasColor0()) {
reader.ReadColor0(unlitColor);
} else {
unlitColor[0] = gstate.getMaterialAmbientR() / 255.f;
unlitColor[1] = gstate.getMaterialAmbientG() / 255.f;
unlitColor[2] = gstate.getMaterialAmbientB() / 255.f;
unlitColor[3] = gstate.getMaterialAmbientA() / 255.f;
}
float litColor0[4];
float litColor1[4];
lighter.Light(litColor0, litColor1, unlitColor, out, normal);
if (gstate.isLightingEnabled()) {
// Don't ignore gstate.lmode - we should send two colors in that case
for (int j = 0; j < 4; j++) {
c0[j] = litColor0[j];
}
if (lmode) {
// Separate colors
for (int j = 0; j < 4; j++) {
c1[j] = litColor1[j];
}
} else {
// Summed color into c0
for (int j = 0; j < 4; j++) {
c0[j] = ((c0[j] + litColor1[j]) > 1.0f) ? 1.0f : (c0[j] + litColor1[j]);
}
}
} else {
if (reader.hasColor0()) {
for (int j = 0; j < 4; j++) {
c0[j] = unlitColor[j];
}
} else {
c0[0] = gstate.getMaterialAmbientR() / 255.f;
c0[1] = gstate.getMaterialAmbientG() / 255.f;
c0[2] = gstate.getMaterialAmbientB() / 255.f;
c0[3] = gstate.getMaterialAmbientA() / 255.f;
}
if (lmode) {
for (int j = 0; j < 4; j++) {
c1[j] = 0.0f;
}
}
}
float ruv[2] = {0.0f, 0.0f};
if (reader.hasUV())
reader.ReadUV(ruv);
// Perform texture coordinate generation after the transform and lighting - one style of UV depends on lights.
switch (gstate.getUVGenMode()) {
case GE_TEXMAP_TEXTURE_COORDS: // UV mapping
case GE_TEXMAP_UNKNOWN: // Seen in Riviera. Unsure of meaning, but this works.
// Texture scale/offset is only performed in this mode.
if (scaleUV) {
uv[0] = ruv[0]*gstate_c.uv.uScale + gstate_c.uv.uOff;
uv[1] = ruv[1]*gstate_c.uv.vScale + gstate_c.uv.vOff;
} else {
uv[0] = ruv[0];
uv[1] = ruv[1];
}
uv[2] = 1.0f;
break;
case GE_TEXMAP_TEXTURE_MATRIX:
{
// Projection mapping
Vec3f source;
switch (gstate.getUVProjMode()) {
case GE_PROJMAP_POSITION: // Use model space XYZ as source
source = pos;
break;
case GE_PROJMAP_UV: // Use unscaled UV as source
source = Vec3f(ruv[0], ruv[1], 0.0f);
break;
case GE_PROJMAP_NORMALIZED_NORMAL: // Use normalized normal as source
if (reader.hasNormal()) {
source = Vec3f(norm).Normalized();
} else {
ERROR_LOG_REPORT(G3D, "Normal projection mapping without normal?");
source = Vec3f(0.0f, 0.0f, 1.0f);
}
break;
case GE_PROJMAP_NORMAL: // Use non-normalized normal as source!
if (reader.hasNormal()) {
source = Vec3f(norm);
} else {
ERROR_LOG_REPORT(G3D, "Normal projection mapping without normal?");
source = Vec3f(0.0f, 0.0f, 1.0f);
}
break;
}
float uvw[3];
Vec3ByMatrix43(uvw, &source.x, gstate.tgenMatrix);
uv[0] = uvw[0];
uv[1] = uvw[1];
uv[2] = uvw[2];
}
break;
case GE_TEXMAP_ENVIRONMENT_MAP:
// Shade mapping - use two light sources to generate U and V.
{
Vec3f lightpos0 = Vec3f(gstate_c.lightpos[gstate.getUVLS0()]).Normalized();
Vec3f lightpos1 = Vec3f(gstate_c.lightpos[gstate.getUVLS1()]).Normalized();
uv[0] = (1.0f + Dot(lightpos0, normal))/2.0f;
uv[1] = (1.0f - Dot(lightpos1, normal))/2.0f;
uv[2] = 1.0f;
}
break;
default:
// Illegal
ERROR_LOG_REPORT(G3D, "Impossible UV gen mode? %d", gstate.getUVGenMode());
break;
}
uv[0] = uv[0] * widthFactor;
uv[1] = uv[1] * heightFactor;
// Transform the coord by the view matrix.
Vec3ByMatrix43(v, out, gstate.viewMatrix);
fogCoef = (v[2] + fog_end) * fog_slope;
}
// TODO: Write to a flexible buffer, we don't always need all four components.
memcpy(&transformed[index].x, v, 3 * sizeof(float));
transformed[index].fog = fogCoef;
memcpy(&transformed[index].u, uv, 3 * sizeof(float));
if (gstate_c.flipTexture) {
transformed[index].v = 1.0f - transformed[index].v;
}
for (int i = 0; i < 4; i++) {
transformed[index].color0[i] = c0[i] * 255.0f;
}
for (int i = 0; i < 3; i++) {
transformed[index].color1[i] = c1[i] * 255.0f;
}
}
// Here's the best opportunity to try to detect rectangles used to clear the screen, and
// replace them with real OpenGL clears. This can provide a speedup on certain mobile chips.
// Disabled for now - depth does not come out exactly the same.
//
// An alternative option is to simply ditch all the verts except the first and last to create a single
// rectangle out of many. Quite a small optimization though.
if (false && maxIndex > 1 && gstate.isModeClear() && prim == GE_PRIM_RECTANGLES && IsReallyAClear(maxIndex)) {
u32 clearColor;
memcpy(&clearColor, transformed[0].color0, 4);
float clearDepth = transformed[0].z;
const float col[4] = {
((clearColor & 0xFF)) / 255.0f,
((clearColor & 0xFF00) >> 8) / 255.0f,
((clearColor & 0xFF0000) >> 16) / 255.0f,
((clearColor & 0xFF000000) >> 24) / 255.0f,
};
bool colorMask = gstate.isClearModeColorMask();
bool alphaMask = gstate.isClearModeAlphaMask();
glstate.colorMask.set(colorMask, colorMask, colorMask, alphaMask);
if (alphaMask) {
glstate.stencilTest.set(true);
// Clear stencil
// TODO: extract the stencilValue properly, see below
int stencilValue = 0;
glstate.stencilFunc.set(GL_ALWAYS, stencilValue, 255);
} else {
// Don't touch stencil
glstate.stencilTest.set(false);
}
glstate.scissorTest.set(false);
bool depthMask = gstate.isClearModeDepthMask();
int target = 0;
if (colorMask || alphaMask) target |= GL_COLOR_BUFFER_BIT | GL_STENCIL_BUFFER_BIT;
if (depthMask) target |= GL_DEPTH_BUFFER_BIT;
glClearColor(col[0], col[1], col[2], col[3]);
#ifdef USING_GLES2
glClearDepthf(clearDepth);
#else
glClearDepth(clearDepth);
#endif
glClearStencil(0); // TODO - take from alpha?
glClear(target);
return;
}
void TransformAndDrawPrim(void *verts, void *inds, int prim, int vertexCount, LinkedShader *program, float *customUV, int forceIndexType)
{
// First, decode the verts and apply morphing
VertexDecoder dec;
dec.SetVertexType(gstate.vertType);
dec.DecodeVerts(decoded, verts, inds, prim, vertexCount);
bool useTexCoord = false;
// Check if anything needs updating
if (gstate.textureChanged)
{
if (gstate.textureMapEnable && !(gstate.clearmode & 1))
{
PSPSetTexture();
useTexCoord = true;
}
}
// Then, transform and draw in one big swoop (urgh!)
// need to move this to the shader.
// We're gonna have to keep software transforming RECTANGLES, unless we use a geom shader which we can't on OpenGL ES 2.0.
// Usually, though, these primitives don't use lighting etc so it's no biggie performance wise, but it would be nice to get rid of
// this code.
// Actually, if we find the camera-relative right and down vectors, it might even be possible to add the extra points in pre-transformed
// space and thus make decent use of hardware transform.
// Actually again, single quads could be drawn more efficiently using GL_TRIANGLE_STRIP, no need to duplicate verts as for
// GL_TRIANGLES. Still need to sw transform to compute the extra two corners though.
// Temporary storage for RECTANGLES emulation
float v2[3] = {0};
float uv2[2] = {0};
int numTrans = 0;
TransformedVertex *trans = &transformed[0];
// TODO: Could use glDrawElements in some cases, see below.
// TODO: Split up into multiple draw calls for Android where you can't guarantee support for more than 0x10000 verts.
int i = 0;
#ifdef ANDROID
if (vertexCount > 0x10000/3)
vertexCount = 0x10000/3;
#endif
for (int i = 0; i < vertexCount; i++)
{
int indexType = (gstate.vertType & GE_VTYPE_IDX_MASK);
if (forceIndexType != -1) {
indexType = forceIndexType;
}
int index;
if (indexType == GE_VTYPE_IDX_8BIT)
{
index = ((u8*)inds)[i];
}
else if (indexType == GE_VTYPE_IDX_16BIT)
{
index = ((u16*)inds)[i];
}
else
{
index = i;
}
float v[3] = {0,0,0};
float c[4] = {1,1,1,1};
float uv[2] = {0,0};
if (gstate.vertType & GE_VTYPE_THROUGH_MASK)
{
// Do not touch the coordinates or the colors. No lighting.
for (int j=0; j<3; j++)
v[j] = decoded[index].pos[j];
// TODO : check if has color
for (int j=0; j<4; j++)
c[j] = decoded[index].color[j];
// TODO : check if has uv
for (int j=0; j<2; j++)
uv[j] = decoded[index].uv[j];
//Rescale UV?
}
else
{
//We do software T&L for now
float out[3], norm[3];
if ((gstate.vertType & GE_VTYPE_WEIGHT_MASK) == GE_VTYPE_WEIGHT_NONE)
{
Vec3ByMatrix43(out, decoded[index].pos, gstate.worldMatrix);
Norm3ByMatrix43(norm, decoded[index].normal, gstate.worldMatrix);
}
else
{
Vec3 psum(0,0,0);
Vec3 nsum(0,0,0);
int nweights = (gstate.vertType & GE_VTYPE_WEIGHT_MASK) >> GE_VTYPE_WEIGHT_SHIFT;
for (int i = 0; i < nweights; i++)
{
Vec3ByMatrix43(out, decoded[index].pos, gstate.boneMatrix+i*12);
Norm3ByMatrix43(norm, decoded[index].normal, gstate.boneMatrix+i*12);
Vec3 tpos(out), tnorm(norm);
psum += tpos*decoded[index].weights[i];
nsum += tnorm*decoded[index].weights[i];
}
nsum.Normalize();
psum.Write(out);
nsum.Write(norm);
}
// Perform lighting here if enabled. don't need to check through, it's checked above.
float dots[4] = {0,0,0,0};
if (program->a_color0 != -1)
{
//c[1] = norm[1];
float litColor[4] = {0,0,0,0};
Light(litColor, decoded[index].color, out, norm, dots);
if (gstate.lightingEnable & 1)
{
memcpy(c, litColor, sizeof(litColor));
}
else
{
// no lighting? copy the color.
for (int j=0; j<4; j++)
c[j] = decoded[index].color[j];
}
}
else
{
// no color in the fragment program???
for (int j=0; j<4; j++)
c[j] = decoded[index].color[j];
}
if (customUV) {
uv[0] = customUV[index * 2 + 0]*gstate.uScale + gstate.uOff;
uv[1] = customUV[index * 2 + 1]*gstate.vScale + gstate.vOff;
} else {
// Perform texture coordinate generation after the transform and lighting - one style of UV depends on lights.
switch (gstate.texmapmode & 0x3)
{
case 0: // UV mapping
// Texture scale/offset is only performed in this mode.
uv[0] = decoded[index].uv[0]*gstate.uScale + gstate.uOff;
uv[1] = decoded[index].uv[1]*gstate.vScale + gstate.vOff;
break;
case 1:
{
// Projection mapping
Vec3 source;
switch ((gstate.texmapmode >> 8) & 0x3)
{
case 0: // Use model space XYZ as source
source = decoded[index].pos;
break;
case 1: // Use unscaled UV as source
source = Vec3(decoded[index].uv[0], decoded[index].uv[1], 0.0f);
break;
case 2: // Use normalized normal as source
source = Vec3(norm).Normalized();
break;
case 3: // Use non-normalized normal as source!
source = Vec3(norm);
break;
}
float uvw[3];
Vec3ByMatrix43(uvw, &source.x, gstate.tgenMatrix);
uv[0] = uvw[0];
uv[1] = uvw[1];
}
break;
case 2:
// Shade mapping
{
int lightsource1 = gstate.texshade & 0x3;
int lightsource2 = (gstate.texshade >> 8) & 0x3;
uv[0] = dots[lightsource1];
uv[1] = dots[lightsource2];
}
break;
case 3:
// Illegal
break;
}
}
// Transform the coord by the view matrix. Should this be done before or after texcoord generation?
Vec3ByMatrix43(v, out, gstate.viewMatrix);
}
// We need to tesselate axis-aligned rectangles, as they're only specified by two coordinates.
if (prim == GE_PRIM_RECTANGLES)
{
if ((i & 1) == 0)
{
// Save this vertex so we can generate when we get the next one. Color is taken from the last vertex.
memcpy(v2, v, sizeof(float)*3);
memcpy(uv2,uv,sizeof(float)*2);
}
else
{
// We have to turn the rectangle into two triangles, so 6 points. Sigh.
// top left
trans->x = v[0]; trans->y = v[1];
trans->z = v[2];
trans->uv[0] = uv[0]; trans->uv[1] = uv[1];
memcpy(trans->color, c, 4*sizeof(float));
trans++;
// top right
trans->x = v2[0]; trans->y = v[1];
trans->z = v[2];
trans->uv[0] = uv2[0]; trans->uv[1] = uv[1];
memcpy(trans->color, c, 4*sizeof(float));
trans++;
// bottom right
trans->x = v2[0]; trans->y = v2[1];
trans->z = v[2];
trans->uv[0] = uv2[0]; trans->uv[1] = uv2[1];
memcpy(trans->color, c, 4*sizeof(float));
trans++;
// bottom left
trans->x = v[0]; trans->y = v2[1];
trans->z = v[2];
trans->uv[0] = uv[0]; trans->uv[1] = uv2[1];
memcpy(trans->color, c, 4*sizeof(float));
trans++;
// top left
trans->x = v[0]; trans->y = v[1];
trans->z = v[2];
trans->uv[0] = uv[0]; trans->uv[1] = uv[1];
memcpy(trans->color, c, 4*sizeof(float));
trans++;
// bottom right
trans->x = v2[0]; trans->y = v2[1];
trans->z = v[2];
trans->uv[0] = uv2[0]; trans->uv[1] = uv2[1];
memcpy(trans->color, c, 4*sizeof(float));
trans++;
numTrans += 6;
}
}
else
{
memcpy(&trans->x, v, 3*sizeof(float));
memcpy(trans->color, c, 4*sizeof(float));
memcpy(trans->uv, uv, 2*sizeof(float));
trans++;
numTrans++;
}
}
glEnableVertexAttribArray(program->a_position);
if (useTexCoord && program->a_texcoord != -1) glEnableVertexAttribArray(program->a_texcoord);
if (program->a_color0 != -1) glEnableVertexAttribArray(program->a_color0);
const int vertexSize = sizeof(*trans);
glVertexAttribPointer(program->a_position, 3, GL_FLOAT, GL_FALSE, vertexSize, transformed);
if (useTexCoord && program->a_texcoord != -1) glVertexAttribPointer(program->a_texcoord, 2, GL_FLOAT, GL_FALSE, vertexSize, ((uint8_t*)transformed) + 3 * 4);
if (program->a_color0 != -1) glVertexAttribPointer(program->a_color0, 4, GL_FLOAT, GL_FALSE, vertexSize, ((uint8_t*)transformed) + 5 * 4);
// NOTICE_LOG(G3D,"DrawPrimitive: %i", numTrans);
glDrawArrays(glprim[prim], 0, numTrans);
glDisableVertexAttribArray(program->a_position);
if (useTexCoord && program->a_texcoord != -1) glDisableVertexAttribArray(program->a_texcoord);
if (program->a_color0 != -1) glDisableVertexAttribArray(program->a_color0);
/*
if (((gstate.vertType ) & GE_VTYPE_IDX_MASK) == GE_VTYPE_IDX_8BIT)
{
glDrawElements(glprim, vertexCount, GL_UNSIGNED_BYTE, inds);
}
else if (((gstate.vertType ) & GE_VTYPE_IDX_MASK) == GE_VTYPE_IDX_16BIT)
{
glDrawElements(glprim, vertexCount, GL_UNSIGNED_SHORT, inds);
}
else
{*/
}
// This normalizes a set of vertices in any format to SimpleVertex format, by processing away morphing AND skinning.
// The rest of the transform pipeline like lighting will go as normal, either hardware or software.
// The implementation is initially a bit inefficient but shouldn't be a big deal.
// An intermediate buffer of not-easy-to-predict size is stored at bufPtr.
u32 TransformDrawEngine::NormalizeVertices(u8 *outPtr, u8 *bufPtr, const u8 *inPtr, VertexDecoder *dec, int lowerBound, int upperBound, u32 vertType) {
// First, decode the vertices into a GPU compatible format. This step can be eliminated but will need a separate
// implementation of the vertex decoder.
dec->DecodeVerts(bufPtr, inPtr, lowerBound, upperBound);
// OK, morphing eliminated but bones still remain to be taken care of.
// Let's do a partial software transform where we only do skinning.
VertexReader reader(bufPtr, dec->GetDecVtxFmt(), vertType);
SimpleVertex *sverts = (SimpleVertex *)outPtr;
const u8 defaultColor[4] = {
(u8)gstate.getMaterialAmbientR(),
(u8)gstate.getMaterialAmbientG(),
(u8)gstate.getMaterialAmbientB(),
(u8)gstate.getMaterialAmbientA(),
};
// Let's have two separate loops, one for non skinning and one for skinning.
if (!g_Config.bSoftwareSkinning && (vertType & GE_VTYPE_WEIGHT_MASK) != GE_VTYPE_WEIGHT_NONE) {
int numBoneWeights = vertTypeGetNumBoneWeights(vertType);
for (int i = lowerBound; i <= upperBound; i++) {
reader.Goto(i);
SimpleVertex &sv = sverts[i];
if (vertType & GE_VTYPE_TC_MASK) {
reader.ReadUV(sv.uv);
}
if (vertType & GE_VTYPE_COL_MASK) {
reader.ReadColor0_8888(sv.color);
} else {
memcpy(sv.color, defaultColor, 4);
}
float nrm[3], pos[3];
float bnrm[3], bpos[3];
if (vertType & GE_VTYPE_NRM_MASK) {
// Normals are generated during tesselation anyway, not sure if any need to supply
reader.ReadNrm(nrm);
} else {
nrm[0] = 0;
nrm[1] = 0;
nrm[2] = 1.0f;
}
reader.ReadPos(pos);
// Apply skinning transform directly
float weights[8];
reader.ReadWeights(weights);
// Skinning
Vec3Packedf psum(0,0,0);
Vec3Packedf nsum(0,0,0);
for (int w = 0; w < numBoneWeights; w++) {
if (weights[w] != 0.0f) {
Vec3ByMatrix43(bpos, pos, gstate.boneMatrix+w*12);
Vec3Packedf tpos(bpos);
psum += tpos * weights[w];
Norm3ByMatrix43(bnrm, nrm, gstate.boneMatrix+w*12);
Vec3Packedf tnorm(bnrm);
nsum += tnorm * weights[w];
}
}
sv.pos = psum;
sv.nrm = nsum;
}
} else {
for (int i = lowerBound; i <= upperBound; i++) {
reader.Goto(i);
SimpleVertex &sv = sverts[i];
if (vertType & GE_VTYPE_TC_MASK) {
reader.ReadUV(sv.uv);
} else {
sv.uv[0] = 0; // This will get filled in during tesselation
sv.uv[1] = 0;
}
if (vertType & GE_VTYPE_COL_MASK) {
reader.ReadColor0_8888(sv.color);
} else {
memcpy(sv.color, defaultColor, 4);
}
if (vertType & GE_VTYPE_NRM_MASK) {
// Normals are generated during tesselation anyway, not sure if any need to supply
reader.ReadNrm((float *)&sv.nrm);
} else {
sv.nrm.x = 0;
sv.nrm.y = 0;
sv.nrm.z = 1.0f;
}
reader.ReadPos((float *)&sv.pos);
}
}
// Okay, there we are! Return the new type (but keep the index bits)
return GE_VTYPE_TC_FLOAT | GE_VTYPE_COL_8888 | GE_VTYPE_NRM_FLOAT | GE_VTYPE_POS_FLOAT | (vertType & (GE_VTYPE_IDX_MASK | GE_VTYPE_THROUGH));
}
int main(int argc, char **argv)
{
#define MAX_BITS (49) // 49-bit word.
#define MAX_DATA (1*1024) // 1 KB data buffer of 49-bit data words - ~70 seconds @ 1 kHz.
char data[MAX_DATA*MAX_BITS], word[MAX_BITS];
//int (*data_ptr)[MAX_DATA][MAX_BITS] = &data, (*bit_ptr)[MAX_BITS] = data;
int i = 0, j = 0, flag = 0, bit_cnt = 0, data_cnt = 0, zones, cmd;
int data0,data1,data2,data3,data4,data5,data6;
char year3[2],year4[2],month[2],day[2],hour[2],minute[2];
FILE *out_file;
struct sched_param param;
struct timespec t, tmark;
char msg[100] = "", oldMsg[100] = "";
struct fifo dataFifo;
/* Declare ourself as a real time task */
param.sched_priority = MY_PRIORITY;
if(sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
perror("sched_setscheduler failed");
exit(-1);
}
/* Lock memory */
if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
perror("mlockall failed");
exit(-2);
}
/* Pre-fault our stack */
stack_prefault();
for(i = 0; i < MAX_DATA*MAX_BITS; i++)
data[i] = '0';
for(i = 0; i < MAX_BITS; i++)
word[i] = '0';
// Set up gpio pointer for direct register access
setup_io();
// Set up FIFO
fifo_init(&dataFifo, data, MAX_DATA*MAX_BITS);
// Set pin direction
INP_GPIO(PI_DATA_OUT); // must use INP_GPIO before we can use OUT_GPIO
OUT_GPIO(PI_DATA_OUT);
INP_GPIO(PI_DATA_IN);
INP_GPIO(PI_CLOCK_IN);
// Set PI_DATA_OUT pin low.
GPIO_CLR = 1<<PI_DATA_OUT;
clock_gettime(CLOCK_MONOTONIC, &t);
tmark = t;
while (1) {
//printf("pi_data:%i,data[%i][%i]:%i\n",GET_GPIO(PI_DATA_IN),data_cnt,bit_cnt,data[data_cnt][bit_cnt]);
t.tv_nsec += INTERVAL;
tnorm(&t);
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
if (GET_GPIO(PI_CLOCK_IN) == PI_CLOCK_HI) flag = 1;
else if ((GET_GPIO(PI_CLOCK_IN) == PI_CLOCK_LO) && (flag == 1)) {
if (ts_diff(&t, &tmark) > 1100000) { // new word
bit_cnt = 0; // start new word
fifo_write(&dataFifo, word, MAX_BITS); // write current word to FIFO
/*printf("%04i: wrote ", data_cnt);
for(i=0; i<MAX_BITS; i++) printf("%c", word[i]);
printf(" to dataFifo\n");*/
data_cnt++;
}
tmark = t;
flag = 0;
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
/*t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);
t.tv_nsec += INTERVAL;
tnorm(&t);*/
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); // wait 50 uS for valid data
word[bit_cnt++] = (GET_GPIO(PI_DATA_IN) == PI_DATA_HI) ? '0' : '1';
}
if (data_cnt == MAX_DATA)
break;
}
// decode and write to file
if ( (out_file = fopen ("data", "w")) == NULL )
printf ("*** data could not be opened. \n" );
else
for ( i = 0; i < MAX_DATA; i++ ) {
strcpy(msg, "");
fifo_read(&dataFifo, word, MAX_BITS);
/*printf("%04i: read ", i);
for(j=0; j<MAX_BITS; j++) printf("%c", word[j]);
printf(" from dataFifo\n");*/
cmd = getBinaryData(word,0,8);
if (cmd == 0x05) {
strcpy(msg, "LED Status: ");
if (getBinaryData(word,12,1)) strcat(msg, "Error ");
if (getBinaryData(word,13,1)) strcat(msg, "Bypass ");
if (getBinaryData(word,14,1)) strcat(msg, "Memory ");
if (getBinaryData(word,15,1)) strcat(msg, "Armed ");
if (getBinaryData(word,16,1)) strcat(msg, "Ready ");
}
else if (cmd == 0xa5) {
sprintf(year3, "%d", getBinaryData(word,9,4));
sprintf(year4, "%d", getBinaryData(word,13,4));
sprintf(month, "%d", getBinaryData(word,19,4));
sprintf(day, "%d", getBinaryData(word,23,5));
sprintf(hour, "%d", getBinaryData(word,28,5));
sprintf(minute, "%d", getBinaryData(word,33,6));
strcpy(msg, "Date: 20");
strcat(msg, year3);
strcat(msg, year4);
strcat(msg, "-");
strcat(msg, month);
strcat(msg, "-");
strcat(msg, day);
strcat(msg, " ");
strcat(msg, hour);
strcat(msg, ":");
strcat(msg, minute);
}
else if (cmd == 0x27) {
strcpy(msg, "Zone1: ");
zones = getBinaryData(word,41,8);
if (zones & 1) strcat(msg, "1 ");
if (zones & 2) strcat(msg, "2 ");
if (zones & 4) strcat(msg, "3 ");
if (zones & 8) strcat(msg, "4 ");
if (zones & 16) strcat(msg, "5 ");
if (zones & 32) strcat(msg, "6 ");
if (zones & 64) strcat(msg, "7 ");
if (zones & 128) strcat(msg, "8 ");
}
else if (cmd == 0x2d) {
strcpy(msg, "Zone2: ");
zones = getBinaryData(word,41,8);
if (zones & 1) strcat(msg, "9 ");
if (zones & 2) strcat(msg, "10 ");
if (zones & 4) strcat(msg, "11 ");
if (zones & 8) strcat(msg, "12 ");
if (zones & 16) strcat(msg, "13 ");
if (zones & 32) strcat(msg, "14 ");
if (zones & 64) strcat(msg, "15 ");
if (zones & 128) strcat(msg, "16 ");
}
else if (cmd == 0x34) {
strcpy(msg, "Zone3: ");
zones = getBinaryData(word,41,8);
if (zones & 1) strcat(msg, "9 ");
if (zones & 2) strcat(msg, "10 ");
if (zones & 4) strcat(msg, "11 ");
if (zones & 8) strcat(msg, "12 ");
if (zones & 16) strcat(msg, "13 ");
if (zones & 32) strcat(msg, "14 ");
if (zones & 64) strcat(msg, "15 ");
if (zones & 128) strcat(msg, "16 ");
}
else if (cmd == 0x3e) {
strcpy(msg, "Zone4: ");
zones = getBinaryData(word,41,8);
if (zones & 1) strcat(msg, "9 ");
if (zones & 2) strcat(msg, "10 ");
if (zones & 4) strcat(msg, "11 ");
if (zones & 8) strcat(msg, "12 ");
if (zones & 16) strcat(msg, "13 ");
if (zones & 32) strcat(msg, "14 ");
if (zones & 64) strcat(msg, "15 ");
if (zones & 128) strcat(msg, "16 ");
}
else
strcpy(msg, "Unknown command.");
data0 = getBinaryData(word,0,8); data1 = getBinaryData(word,8,8);
data2 = getBinaryData(word,16,8); data3 = getBinaryData(word,24,8);
data4 = getBinaryData(word,32,8); data5 = getBinaryData(word,40,8);
data6 = getBinaryData(word,48,2);
if (strcmp(msg, oldMsg) != 0) {
fprintf (out_file, "data[%i],cmd:0x%02x,%s\n", i, cmd, msg);
fprintf (out_file, "***data-all: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",data0,data1,data2,data3,data4,data5,data6);
strcpy(oldMsg, msg);
}
}
fclose (out_file);
/* Unlock memory */
if(munlockall() == -1) {
perror("munlockall failed");
exit(-2);
}
return 0;
} // main
double RNG::tnorm(double left, double right)
{
// The most difficult part of this algorithm is figuring out all the
// various cases. An outline is summarized in the Appendix.
// Check input
#ifdef USE_R
if (ISNAN(right) || ISNAN(left))
#else
if (std::isnan(right) || std::isnan(left))
#endif
{
fprintf(stderr, "Warning: nan sent to RNG::tnorm: left=%g, right=%g\n", left, right);
TREOR("RNG::tnorm: parameter problem.\n", 0.5 * (left + right));
// throw std::runtime_error("RNG::tnorm: parameter problem.\n");
}
if (right < left) {
fprintf(stderr, "Warning: left: %g, right:%g.\n", left, right);
TREOR("RNG::tnorm: parameter problem.\n", 0.5 * (left + right));
}
double rho, ppsl;
int count = 1;
if (left >= 0) {
double lbound = lowerbound(left);
if (right > lbound) { // Truncated Exponential.
double astar = alphastar(left);
while (true) {
ppsl = texpon_rate(left, right, astar);
rho = exp(-0.5*(ppsl - astar)*(ppsl-astar));
if (unif() < rho) return ppsl;
if (count > RCHECK * 10) fprintf(stderr, "left >= 0, right > lbound; count: %i\n", count);
// if (ppsl < right) return ppsl;
}
}
else {
while (true) {
ppsl = flat(left, right);
rho = exp(0.5 * (left*left - ppsl*ppsl));
if (unif() < rho) return ppsl;
check_R_interupt(count++);
#ifndef NDEBUG
if (count > RCHECK * 10) fprintf(stderr, "left >= 0, right <= lbound; count: %i\n", count);
#endif
}
}
}
else if (right >= 0) {
if ( (right - left) < SQRT2PI ){
while (true) {
ppsl = flat(left, right);
rho = exp(-0.5 * ppsl * ppsl);
if (unif() < rho) return ppsl;
check_R_interupt(count++);
#ifndef NDEBUG
if (count > RCHECK * 10) fprintf(stderr, "First, left < 0, right >= 0, count: %i\n", count);
#endif
}
}
else{
while (true) {
ppsl = norm(0, 1);
if (left < ppsl && ppsl < right) return ppsl;
check_R_interupt(count++);
#ifndef NDEBUG
if (count > RCHECK * 10) fprintf(stderr, "Second, left < 0, right > 0, count: %i\n", count);
#endif
}
}
}
else {
return -1. * tnorm(-1.0 * right, -1.0 * left);
}
} // tnorm
double RNG::tnorm(double left, double mu, double sd)
{
double newleft = (left - mu) / sd;
return mu + tnorm(newleft) * sd;
} // tnorm
