Square::Square( vect3d &pCenter,
vect3d &pNormal,
vect3d &pHeadVec,
float nWidth,
float nHeight,
float fReflR, float fRefrR, float fRefrK, float fEmitR)
:PrimaryObject(fReflR, fRefrR, fRefrK, fEmitR)
{
vecCopy(_vCenter, pCenter);
vecCopy(_vNormal, pNormal);
vecCopy(_vWidthVec, pHeadVec);
_nWidth = nWidth;
_nHeight = nHeight;
vecScale(_vWidthVec, nHeight/2, _v2WidthVec);
vect3d v2HeightVec;
cross_product(_vWidthVec, _vNormal, v2HeightVec);
normalize(v2HeightVec);
vecScale(v2HeightVec, nWidth/2, _v2HeightVec);
a = _vNormal[0]; b = _vNormal[1]; c = _vNormal[2];
d = (-1) * (a * _vCenter[0] + b * _vCenter[1] + c * _vCenter[2]);
updateBBox();
}
Camera::Camera(vect3d &pCtrPos, vect3d &pUpVec, vect3d &pViewVec, float fNtoF, float fPlaneRatio)
: _fN2F(fNtoF), _fPlaneRatio(fPlaneRatio), _pSampler(NULL)
{
assert((fNtoF > 0) && (fPlaneRatio > 0));
// viewing direction
_dir[0] = pViewVec[0];
_dir[1] = pViewVec[1];
_dir[2] = pViewVec[2];
normalize(_dir);
// Center point of the view plane
_ctrPos[0] = pCtrPos[0];
_ctrPos[1] = pCtrPos[1];
_ctrPos[2] = pCtrPos[2];
// Up vec of the view plane, with vec-len as 1/2 of view plane height
_upVec[0] = pUpVec[0];
_upVec[1] = pUpVec[1];
_upVec[2] = pUpVec[2];
normalize(_upVec);
vecScale(_upVec, WinHeight * _fPlaneRatio * 0.5, _upVec);
// right vec of the view plane, with vec-len as 1/2 of view plane width
cross_product(_dir, _upVec, _rightVec);
normalize(_rightVec);
vecScale(_rightVec, WinWidth * _fPlaneRatio * 0.5, _rightVec);
// default: no multi-sampling
_nMultiSamplingCount = 1;
}
void PerpCamera::genViewRaysByRow(unsigned nRowCount, PixelIntegrator *pBuf)
{
// TODO: the following code piece is redundant...
//
assert(pBuf && nRowCount < WinHeight && _pSampler);
for(unsigned i = 0; i < WinWidth; i ++) // loop for an entire row
{
// to fine the current primary ray starting point
//
vect3d nCurrCtr;
// right vec first
vect3d rightVec;
point2point(rightVec, _rightVec, rightVec);
vecScale(rightVec, (i - WinWidth/2.f) * ViewPlaneRatio / vecLen(_rightVec), rightVec);
// up vec second
vect3d upVec;
point2point(upVec, _upVec, upVec);
vecScale(upVec, (nRowCount - WinHeight/2.f) * ViewPlaneRatio/ vecLen(_upVec), upVec);
point2point(_ctrPos, rightVec, nCurrCtr);
point2point(nCurrCtr, upVec, nCurrCtr);
// since we reuse the vector, we have to clear it before next use
(pBuf + i)->getRays().clear();
for(unsigned j = 0; j < _nMultiSamplingCount; j ++)
{
vect3d startPoint;
// Get current sampling start point
float fdx = 0, fdy = 0;
_pSampler->getNextSample(&fdx, &fdy);
assert( (fdx >= -1 && fdx <= 1) &&
(fdy >= -1 && fdy <= 1) );
vect3d vDeltaXVec;
vecScale(_rightVec, ViewPlaneRatio * fSamplingDeltaFactor * fdx, vDeltaXVec);
vect3d vDeltaYVec;
vecScale(_upVec, ViewPlaneRatio * fSamplingDeltaFactor * fdy, vDeltaYVec);
point2point(nCurrCtr, vDeltaXVec, startPoint);
point2point(startPoint, vDeltaYVec, startPoint);
// put into PixelIntegrator
vect3d viewDir;
points2vec(_eyePos, startPoint, viewDir);
normalize(viewDir);
Ray ray(startPoint, viewDir);
ray.fDeltaX = fdx * fSamplingDeltaFactor;
ray.fDeltaY = fdy * fSamplingDeltaFactor;
(pBuf + i)->addRay(ray);
}
}
}
void updateAppendage(Scene* scene, int sceneIndex, cl_float3 p, cl_float3 q,
cl_float3 w, float A, float B, int countA, int countB) {
cl_float3 U;
cl_float3 V;
cl_float3 W;
cl_float3 j;
cl_float3 d;
V = q;
vecSub(V, p);
float D = vecLength(V);
float inverseD = 1.0f / D;
vecScale(V, inverseD);
W = w;
vecNormalize(W);
vecCross(U, V, W);
float A2 = A*A;
float y = 0.5f * inverseD * (A2 - B * B + D * D);
double square = A2 - y*y;
if (square < 0.0f)
throw std::runtime_error("Unable to construct appendage");
float x = sqrtf(square);
j = p;
vecScaledAdd(j, x, U);
vecScaledAdd(j, y, V);
d = j;
vecSub(d, p);
vecScale(d, 1.0f / float(countA));
for (int i = 0; i <= countA; i++) {
Sphere* sphere = &(scene->spheres[sceneIndex + i]);
sphere->center = p;
vecScaledAdd(sphere->center, float(i), d);
vecScale(sphere->center, 0.1f);
}
d = j;
vecSub(d, q);
vecScale(d, 1.0f / float(countB));
for (int i = 0; i <= countA; i++) {
Sphere* sphere = &(scene->spheres[sceneIndex + i + countA + 1]);
sphere->center = q;
vecScaledAdd(sphere->center, float(i), d);
vecScale(sphere->center, 0.1f);
}
}
bool Sphere::isHit( Ray &ray, vect3d &pNormal, float *pt, vect3d *pTexColor)
{
if(!_bbox.isHit(ray))
{
return false;
}
float t, len;
len = len2Line(ray, &t);
if(len > _fRad)
{
return false;
}
if( len <= _fRad)
{
float d = sqrt(_fRad * _fRad - len * len);
float r = d / vecLen(ray.direction_vec);
if(ray.bIsInObj)
{
t += r;
}
else
{
t -= r;
}
}
*pt = t;
// calc normal
vect3d pPoint, vec;
point2point(pPoint, ray.start_point, pPoint);
point2point(vec, ray.direction_vec, vec); vecScale(vec, t, vec);
point2point(pPoint, vec, pPoint);
points2vec(_ctr, pPoint, pNormal);
normalize(pNormal);
// Texture
if(_tex != NULL && pTexColor != NULL)
{
float x = pPoint[0] - _ctr[0];
float y = pPoint[1] - _ctr[1];
float z = pPoint[2] - _ctr[2];
float a = atan2f( x, z );
float b = acosf( y / _fRad );
float u = a / (2 * PI) ;
float v = b / (1 * PI);
//
_tex->getTexAt( (u + 0.5) * _tex->nWidth, (v) * _tex->nHeight, *pTexColor);
}
return true;
}
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);
}
PerpCamera::PerpCamera(float fEye2Near, vect3d &pCtrPos, vect3d &pUpVec, vect3d &pViewVec, float fNtoF, float fPlaneRatio)
: Camera(pCtrPos, pUpVec, pViewVec, fNtoF, fPlaneRatio)
{
assert(fEye2Near > 0);
//
vect3d vInverseVec;
vecScale(_dir, -fEye2Near, vInverseVec);
point2point(_ctrPos, vInverseVec, _eyePos);
}
i32 CreateRect2D(Resources::CMesh* pMesh, const IInputLayout* pIL, const SRect2DOptions& Opts)
{
Vec3 vecScale( 100 );
Vec2 vecWidth( 0, 100 );
Vec2 vecHeight( 0, 100 );
IndexBufferPtr pIB = pMesh->CreateIndexBuffer();
VertexBufferPtr pVB = pMesh->CreateVertexBuffer();
pVB->SetVertexCount( 4 );
pVB->SetInputLayout( pIL );
pVB->SetTopologyType( TopologyTypes::TRIANGLE_LIST );
pVB->SetUsage( Opts.eUsage );
pIB->SetIndexCount( 6 );
pIB->SetUsage( BufferUsages::STATIC );
pVB->Lock();
CVertexData& VData = pVB->GetVertexData();
if( pIL->IsPosition() )
{
VData.SetPosition( 0, Vec3( 0, 0, 0 ) );
VData.SetPosition( 1, Vec3( vecScale.x, 0, 0 ) );
VData.SetPosition( 2, Vec3( vecScale.x, -vecScale.y, 0 ) );
VData.SetPosition( 3, Vec3( 0, -vecScale.y, 0) );
}
if( pIL->IsColor() )
{
VData.SetColor( 0, Vec4( 1, 1, 1, 1 ) );
VData.SetColor( 1, Vec4( 1, 1, 1, 1 ) );
VData.SetColor( 2, Vec4( 1, 1, 1, 1 ) );
VData.SetColor( 3, Vec4( 1, 1, 1, 1 ) );
}
if( pIL->IsTexCoord0() )
{
VData.SetTexCoord( 0, 0, Vec2( 0, 1 ) );
VData.SetTexCoord( 1, 0, Vec2( 1, 1 ) );
VData.SetTexCoord( 2, 0, Vec2( 0, 1 ) );
VData.SetTexCoord( 3, 0, Vec2( 0, 0 ) );
}
pVB->Unlock();
pVB->Create();
pIB->Lock();
CIndexData& IData = pIB->GetIndexData();
IData.SetTriangle( 0, 0, 2, 3 );
IData.SetTriangle( 1, 1, 2, 0 );
return XST_OK;
}
void setTargetAngVel(vec targetAngVel) {
vec torques;
mathVecSubtract(torques, targetAngVel, stateAngVel(myState), 3);
vecScale(torques, floatOne / accPerNM);
#if printDebug
DEBUG(("Angular Velocity Targeting Info\n"));
printVec("\tTarget AngVel (rad/s)", targetAngVel);
printVec("\tTorques (rad/s^2)", torques);
#endif
api.setTorques(torques);
}
bool Triangle::isHit(Ray &ray, vect3d &pNormal, float *pt, vect3d *pTexColor)
{
// This will slow down the performance
//
//if(!_bbox.isHit(ray))
//{
// return false;
//}
float u = 0, v = 0;
if(isTriangleHit(_vertices, ray, pt, &u, &v))
{
if(_bSmooth && _bHasVNorm)
{
vect3d vSmoothNorm;
point2point(_vnormal[1], vSmoothNorm, vSmoothNorm);
vecScale(vSmoothNorm, u, vSmoothNorm);
vect3d vnorm2;
point2point(_vnormal[2], vnorm2, vnorm2);
vecScale(vnorm2, v, vnorm2);
vect3d vnorm3;
point2point(_vnormal[0], vnorm3, vnorm3);
vecScale(vnorm3, (1 - u - v), vnorm3);
point2point(vSmoothNorm, vnorm2, vSmoothNorm);
point2point(vSmoothNorm, vnorm3, pNormal);
normalize(pNormal);
}
else
{
vecCopy(pNormal, _normal);
}
return true;
}
return false;
}
void getAvgNormal(float *retNorm, float *depthBuf, int x, int y, int width, int height, int nSampleRad)
{
float tmp[3] = {0};
unsigned nAvailableCount = 0;
for(int j = -nSampleRad; j <= nSampleRad; j ++)
for(int i = -nSampleRad; i <= nSampleRad; i ++)
{
int currX = x + i;
int currY = y + j;
if( currX >=0 && currX < width &&
currY >=0 && currY < height )
{
float fd = *(depthBuf + (currX + currY * width));
if(fd > 0)
{
float currNorm[3];
// The sampled normal should get an average value ...
float dz_x = dz2x(currX, currY, width, height, depthBuf);
float dz_y = dz2y(currX, currY, width, height, depthBuf);
float Cx = i == 0 ? 0 : 2.f / (width * currX);
float Cy = j == 0 ? 0 : 2.f / (height * currY);
float D = Cy * Cy * dz_x * dz_x + Cx * Cx * dz_y * dz_y + Cx * Cx * Cy * Cy * (1 - fd) * (1 - fd);
if(D == 0)
{
continue;
}
float rv_sqrtD = 1.f / sqrt(D);
currNorm[0] = - Cy * dz_x * rv_sqrtD;
currNorm[1] = - Cx * dz_y * rv_sqrtD;
currNorm[2] = Cx * Cy * (1 - fd) * rv_sqrtD;
vecAdd(tmp, currNorm, tmp);
nAvailableCount ++;
}
}
}
if(nAvailableCount > 0)
{
vecScale(tmp, 1.f/nAvailableCount, tmp);
normalize(tmp);
vecCopy(retNorm, tmp);
}
}
/* take the definition of a camera and a point within the film plane of the camera and generate a RAY
x and y both have a range of -0.5 to +0.5 */
Ray *camGenerateRay(Camera *camera, double x, double y, Ray *ray){
Vec direction;
Vec cameraX, cameraY;
/* work out direction camera is pointing in */
vecSub(&(camera->lookAt), &(camera->at), &direction);
vecNormalise(&direction, &direction);
/* using that and DOWN, work out the camera axes and normalise */
vecProduct(&direction, &(camera->down), &cameraX);
vecProduct(&direction, &cameraX, &cameraY);
vecNormalise(&cameraX, &cameraX);
vecNormalise(&cameraY, &cameraY);
/* finally combine film offset and camera axes to work out film position */
vecScale(x * camera->width, &cameraX, &cameraX);
vecScale(y * camera->height, &cameraY, &cameraY);
vecScale(camera->depth, &direction, &direction);
vecAdd(&cameraX, &direction, &direction);
vecAdd(&cameraY, &direction, &direction);
rayInit(&(camera->at), &direction, ray);
return ray;
}
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 FUNC_CALL_TYPE sphi2DecartXZ(float4 dec, int32 lon, int32 lat, float rad) {
float sinLon, cosLon;
float sinLat, cosLat;
float lonf = D_SPHI2RAD(lon);
float latf = D_SPHI2RAD(lat);
#if !defined (D_USE_FSINCOS)
deadlySinCosEx(&sinLon, &cosLon, lonf);
deadlySinCosEx(&sinLat, &cosLat, latf);
#else
fSinCos(&sinLon, &cosLon, lonf);
fSinCos(&sinLat, &cosLat, latf);
#endif
dec[0] = sinLon * cosLat;
dec[1] = sinLat;
dec[2] = cosLon * cosLat;
vecScale(dec, dec, rad);
dec[3] = 1.0f;
}
void animatePositions(Scene* scene, const bool updateCam) {
if (updateCam)
updateCamera(scene);
// update sphere positions
const float JUGGLE_X0 = -182.0f;
const float JUGGLE_X1 = -108.0f;
const float JUGGLE_Y0 = 88.0f;
const float JUGGLE_H_Y = 184.0f;
const float JUGGLE_H_VX = (JUGGLE_X0 - JUGGLE_X1) / 60.0f;
const float JUGGLE_L_VX = (JUGGLE_X1 - JUGGLE_X0) / 30.0f;
const float JUGGLE_H_H = JUGGLE_H_Y - JUGGLE_Y0;
const float JUGGLE_H_VY = 4.0f * JUGGLE_H_H / 60.0f;
const float JUGGLE_G = JUGGLE_H_VY * JUGGLE_H_VY / (2.0f * JUGGLE_H_H);
const float JUGGLE_L_VY = 0.5f * JUGGLE_G * 30.0f;
const float HIPS_MAX_Y = 85.0f;
const float HIPS_MIN_Y = 81.0f;
const float HIPS_ANGLE_MULTIPLIER = 2.0f * M_PI / 30.0f;
double time = WallClockTime() - sceneTimeOffset;
time -= floor(time);
// mirrored juggling balls
float t = 30.0f + 30.0f * float(time);
Sphere* sphere = &(scene->spheres[1]);
sphere->center.s[2] = 0.1f * (JUGGLE_X1 + JUGGLE_H_VX * t);
sphere->center.s[1] = 0.1f * (JUGGLE_Y0 + (JUGGLE_H_VY - 0.5f * JUGGLE_G * t) * t);
t -= 30.0f;
sphere = &(scene->spheres[2]);
sphere->center.s[2] = 0.1f * (JUGGLE_X1 + JUGGLE_H_VX * t);
sphere->center.s[1] = 0.1f * (JUGGLE_Y0 + (JUGGLE_H_VY - 0.5f * JUGGLE_G * t) * t);
sphere = &(scene->spheres[0]);
sphere->center.s[2] = 0.1f * (JUGGLE_X0 + JUGGLE_L_VX * t);
sphere->center.s[1] = 0.1f * (JUGGLE_Y0 + (JUGGLE_L_VY - 0.5f * JUGGLE_G * t) * t);
// body (hips to chest) 3 .. 10
float angle = HIPS_ANGLE_MULTIPLIER*t;
float oscillation = 0.5f * (1.0f + cosf(angle));
cl_float3 o;
vecInit(o, 151.0f,
HIPS_MIN_Y + (HIPS_MAX_Y - HIPS_MIN_Y) * oscillation,
-151.0f);
cl_float3 v;
vecInit(v, 0.0f, 70.0f, (HIPS_MIN_Y - HIPS_MAX_Y) * sinf(angle));
vecNormalize(v);
cl_float3 u;
vecInit(u, 0.0f, v.s[2], -v.s[1]);
cl_float3 w;
vecInit(w, 1.0f, 0.0f, 0.0f);
for (int i = 3; i <= 10; i++) {
float fraction = float(i - 3) / 7.0f;
sphere = &(scene->spheres[i]);
sphere->center = o;
vecScaledAdd(sphere->center, 32.0f * fraction, v);
vecScale(sphere->center, 0.1f);
}
// head
sphere = &(scene->spheres[11]);
sphere->center = o;
vecScaledAdd(sphere->center, 70.0f, v);
vecScale(sphere->center, 0.1f);
// neck
sphere = &(scene->spheres[12]);
sphere->center = o;
vecScaledAdd(sphere->center, 55.0f, v);
vecScale(sphere->center, 0.1f);
// left leg (13 .. 29)
cl_float3 p;
vecInit(p, 159.0f, 2.5f, -133.0f);
cl_float3 q = o;
vecScaledAdd(q, -9.0f, v);
vecScaledAdd(q, -16.0f, u);
updateAppendage(scene, 13, p, q, u, 42.58f, 34.07f, 8, 8);
// right leg (30 .. 46)
vecInit(p, 139.0f, 2.5f, -164.0f);
q = o;
vecScaledAdd(q, -9.0f, v);
vecScaledAdd(q, 16.0f, u);
updateAppendage(scene, 30, p, q, u, 42.58f, 34.07f, 8, 8);
// left arm (47 .. 63)
cl_float3 n;
float armAngle = -0.35f * oscillation;
vecInit(p, 69.0f + 41.0f * cosf(armAngle),
60.0f - 41.0f * sinf(armAngle),
-108.0f);
q = o;
vecScaledAdd(q, 45.0f, v);
vecScaledAdd(q, -19.0f, u);
n = o;
vecScaledAdd(n, 45.41217f, v);
vecScaledAdd(n, -19.91111f, u);
vecSub(n, q);
updateAppendage(scene, 47, p, q, n, 44.294f, 46.098f, 8, 8);
// right arm (64 .. 80)
p.s[2] = -182.0f;
q = o;
vecScaledAdd(q, 45.0f, v);
vecScaledAdd(q, 19.0f, u);
n = o;
vecScaledAdd(n, 45.41217f, v);
vecScaledAdd(n, 19.91111f, u);
vecSub(n, q);
vecScale(n, -1.0f);
updateAppendage(scene, 64, p, q, n, 44.294f, 46.098f, 8, 8);
// left eye (81)
sphere = &(scene->spheres[81]);
sphere->center = o;
vecScaledAdd(sphere->center, 69.0f, v);
vecScaledAdd(sphere->center, -7.0f, u);
sphere->center.s[0] = 142.0f;
vecScale(sphere->center, 0.1f);
// left eye (82)
sphere = &(scene->spheres[82]);
sphere->center = o;
vecScaledAdd(sphere->center, 69.0f, v);
vecScaledAdd(sphere->center, 7.0f, u);
sphere->center.s[0] = 142.0f;
vecScale(sphere->center, 0.1f);
// hair (83)
sphere = &(scene->spheres[83]);
sphere->center = o;
vecScaledAdd(sphere->center, 71.0f, v);
sphere->center.s[0] = 152.0f;
vecScale(sphere->center, 0.1f);
}
Cube::Cube( float fLen, float fWidth, float fHeight,
vect3d &pCenterPos,
vect3d &pVerticalVec,
vect3d &pHorizonVec,
float fReflR, float fRefrR, float fRefrK, float fEmitR)
:PrimaryObject(fReflR, fRefrR, fRefrK, fEmitR)
{
assert( (fLen > 0) && (fWidth > 0) && (fHeight > 0) );
///
/// Since there's no ObjObject in GPU, and I want to make primaryObj id the same as the
/// GPU primaryObj array index, I do this. This should not impact the current functional
/// code.
///
nCurrObj --;
_fLength = fLen;
_fWidth = fWidth;
_fHeight = fHeight;
vecCopy(_vCenterPos, pCenterPos);
vecCopy(_verticalVec, pVerticalVec);
vecCopy(_horizonVec, pHorizonVec);
vect3d tmpVec, tmpPoint;
// Top square
vecScale(_verticalVec, _fHeight / 2.0, tmpVec);
point2point(_vCenterPos, tmpVec, tmpPoint);
_vs[0] = new Square(tmpPoint, _verticalVec, _horizonVec, _fLength, _fWidth);
//_vs[0]->setColor(c1,c1,c1, c1, 0.5);
vecScale(_verticalVec, (-1)*_fHeight / 2.0, tmpVec);
point2point(_vCenterPos, tmpVec, tmpPoint);
vecScale(_verticalVec, -1, tmpVec);
_vs[1] = new Square(tmpPoint, tmpVec, _horizonVec, _fLength, _fWidth);
//_vs[1]->setColor(c1,c1,c1, c1,0.5);
// Left square
vect3d vLeftNormalVec;
cross_product(_horizonVec, _verticalVec, vLeftNormalVec);
normalize(vLeftNormalVec);
vecScale(vLeftNormalVec, _fLength / 2.0, tmpVec);
point2point(_vCenterPos, tmpVec, tmpPoint);
_vs[2] = new Square(tmpPoint, vLeftNormalVec, _horizonVec, _fHeight, _fWidth);
//_vs[2]->setColor(c2,c2,c2, c2,0.5);
vecScale(vLeftNormalVec, (-1)*_fLength / 2.0, tmpVec);
point2point(_vCenterPos, tmpVec, tmpPoint);
vecScale(vLeftNormalVec, -1, tmpVec);
_vs[3] = new Square(tmpPoint, tmpVec, _horizonVec, _fHeight, _fWidth);
//_vs[3]->setColor(c2,c2,c2, c2,0.5);
// Right square
vecScale(_horizonVec, _fWidth / 2.0, tmpVec);
point2point(_vCenterPos, tmpVec, tmpPoint);
_vs[4] = new Square(tmpPoint, _horizonVec, _verticalVec, _fLength, _fHeight);
//_vs[4]->setColor(c3,c3,c3, c3,0.5);
vecScale(_horizonVec, (-1)*_fWidth / 2.0, tmpVec);
point2point(_vCenterPos, tmpVec, tmpPoint);
vecScale(_horizonVec, -1, tmpVec);
_vs[5] = new Square(tmpPoint, tmpVec, _verticalVec, _fLength, _fHeight);
//_vs[5]->setColor(c3,c3,c3, c3,0.5);
updateBBox();
}
bool Square::isHit(Ray &ray, vect3d &pNormal, float *pt, vect3d *pTexColor)
{
if(!_bbox.isHit(ray))
{
return false;
}
// The hit point on the plane
vect3d op;
points2vec(_vCenter, ray.start_point, op);
float dn = dot_product(ray.direction_vec, _vNormal);
if(dn == 0.f)
{
return false;
}
float t = - dot_product(op, _vNormal) / dn;
// NOTE: since it is a 0-thickness plane, we need this.
if(t <= epsi)
{
return false;
}
// Get the hit point
vect3d vHitPoint;
vect3d pView; vecScale(ray.direction_vec, t, pView);
point2point(ray.start_point, pView, vHitPoint);
vect3d vHitVec;
points2vec(vHitPoint, _vCenter, vHitVec);
float dx = dot_product(vHitVec, _v2HeightVec) / pow( _nWidth /2 , 2);
float dy = dot_product(vHitVec, _v2WidthVec) / pow( _nHeight /2 , 2);
if( fabs(dy) <= 1.f && fabs(dx) <= 1.0f )
{
*pt = t;
vecCopy(pNormal, _vNormal);
if(_tex != NULL && pTexColor != NULL)
{
float fWidInx = (-dx + 1.f) / 2.f * _nWidth;
float fHeiInx = (dy + 1.f) / 2.f * _nHeight;
switch(_eTexMapType)
{
case STRETCH:
_tex->getTexAt( fWidInx / _nWidth * _tex->nWidth,
fHeiInx / _nHeight * _tex->nHeight, *pTexColor);
break;
case REPEAT:
_tex->getTexAt( (unsigned)fWidInx % _tex->nWidth,
(unsigned)fHeiInx % _tex->nHeight, *pTexColor);
break;
case STRAIGHT:
if(fWidInx < _tex->nWidth && fHeiInx < _tex->nHeight)
{
_tex->getTexAt( (unsigned)fWidInx, (unsigned)fHeiInx, *pTexColor);
}
break;
};
}
return true;
}
return false;
}
void calcAllNormals(float *normals, float *depthBuf, int width, int height, unsigned nSampleStep)
{
// Grid Points
for(int j = 0; j < height; j += nSampleStep)
for(int i = 0; i < width; i += nSampleStep)
{
float fd = *(depthBuf + (i + j * width));
if(fd > 0)
{
getAvgNormal(normals + 3 * (i + j * width), depthBuf, i, j, width, height, nAvgNormRad);
}
}
if(nSampleStep == 1)
{
return;
}
// Interpolation other pixels
for(int j = 0; j < height; j += 1)
for(int i = 0; i < width; i += 1)
{
if( (i % nSampleStep == 0) && (j % nSampleStep == 0))
{
continue;
}
float fd = *(depthBuf + (i + j * width));
if(fd > 0)
{
// 1. find grid
int nGridX0 = (i / nSampleStep) * nSampleStep;
int nGridY0 = (j / nSampleStep) * nSampleStep;
float fDistPercX = (i - nGridX0) * 1.f / nSampleStep;
float fDistPercY = (j - nGridY0) * 1.f / nSampleStep;
// 2. Bi-linear Interpolation
float NullNorm[3] = {0};
float *currNormals[4];
currNormals[0] = normals + 3 * (nGridX0 + nGridY0 * width);
if((i + 1) < width && (nGridX0 + nSampleStep) < width )
{
currNormals[1] = normals + 3 * (nGridX0 + nSampleStep + nGridY0 * width);
}
else
{
currNormals[1] = NullNorm;
}
if((j + 1)< height && (nGridY0 + nSampleStep) < height)
{
currNormals[2] = normals + 3 * (nGridX0 + (nGridY0 + nSampleStep) * width);
}
else
{
currNormals[2] = NullNorm;
}
if( (i + 1) < width && (j + 1)< height &&
(nGridY0 + nSampleStep) < height &&
(nGridX0 + nSampleStep) < width )
{
currNormals[3] = normals + 3 * (nGridX0 + nSampleStep + (nGridY0 + nSampleStep) * width);
}
else
{
currNormals[3] = NullNorm;
}
float tmp[3], tmp1[3];
float n01[3] = {0};
vecScale(currNormals[0], (1 - fDistPercX), tmp);
vecScale(currNormals[1], fDistPercX, tmp1);
vecAdd(tmp, tmp1, n01);
float n23[3] = {0};
vecScale(currNormals[2], (1 - fDistPercX), tmp);
vecScale(currNormals[3], fDistPercX, tmp1);
vecAdd(tmp, tmp1, n23);
float *pCurrNorm = normals + 3 * (i + j * width);
vecScale(n01, (1 - fDistPercY), tmp);
vecScale(n23, fDistPercY, tmp1);
vecAdd(tmp, tmp1, pCurrNorm);
}
}// for
}
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
vec att = {floatZero, -1.0f, floatZero};
if (!obsCreated) {
if (makeObs(red ? -0.17f : 0.17f, -0.63f)) obsCreated ++;
}
else if (obsCreated == 1) {
if (makeObs(red ? -0.47f : 0.47f, -0.03f)) obsCreated ++;
}
else if (phase == 1) {
vec temp = {norm((red ? -0.45f : 0.45f) - myState[0], 0.015f), 0.05f, floatZero};
setVelocity(temp);
//api.setAttitudeTarget(att);
}
else {
if (!game.haveObject(0)) getItem(0);
else if (!game.haveObject(1) && !game.otherHasObject(1)) getItem(1);
else if (!game.haveObject(2) && !game.otherHasObject(2) && !game.haveObject(1)) getItem(2);
else {
if (game.haveObject(1) || game.haveObject(2)) {
api.setAttitudeTarget(att);
}
vec temp = {floatZero, floatZero, floatZero};
if (phase == 2) {
vec temp2 = {(red ? 0.04f : -0.04f), 0.09f, floatZero};
attToTarget(myState, temp2, temp);
vecScale(temp, 0.057f);
temp[2] = maintainZ();
setVelocity(temp);
}
else {
if (!alreadySet) {
alreadySet = true;
top = myState[2] > floatZero;
}
if (myState[1] > -0.4f) {
remDust();
temp[0] = norm((red ? 0.045f : -0.045f) - myState[0], 0.015f);
temp[1] = -0.051f;
temp[2] = maintainZ();
}
else if (myState[1] > -0.6f) {
temp[0] = norm((red ? 0.045f : -0.045f) - myState[0], 0.015f);
temp[1] = -0.022f;
temp[2] = maintainZ();
}
else {
temp[0] = norm((red ? 0.045f : -0.045f) - myState[0], 0.015f);
temp[1] = floatZero;
temp[2] = (top ? -0.055f : 0.055f);
DEBUG(("Top:\t%d\t%.3f\n", top, temp[2]));
}
setVelocity(temp);
}
}
}
}
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;
}
void ceos_read_stVecs(const char *fName, ceos_description *ceos, meta_parameters *meta)
{
int i;
double timeStart=0.0;/*Time of first state vector, relative to image start.*/
int areInertial=1;/*Flag: are state vectors in non-rotating frame?*/
int areInertialVelocity=0;/*Flag: have state vectors not been corrected for non-rotating frame?*/
int areFixedVelocity=0;/*Flag: were state vectors in fixed-earth velocity; but inertial coordinates?*/
meta_state_vectors *s;
struct pos_data_rec ppdr;
/*Fetch platform position data record.*/
get_ppdr(fName,&ppdr);
/*Allocate output record.*/
meta->state_vectors = meta_state_vectors_init(ppdr.ndata);
meta->state_vectors->vector_count = ppdr.ndata;
s = meta->state_vectors;
/*Determine State Vector Format.*/
if (0==strncmp(ppdr.ref_coord,"INERTIAL",9))
areInertial=1;/*Listed as Inertial-- believe it.*/
if (0==strncmp(ppdr.ref_coord,"EARTH CENTERED ROT",18))
areInertial=0;/*Listed as rotating-- believe it.*/
else if (0==strncmp(ppdr.ref_coord,"Earth Centred Rot",17))
areInertial = 0;
else if (0 == strncmp(ppdr.ref_coord, "ECR", 3)) {
areInertial = 0;
//areInertialVelocity = 1;
}
else if (0 == strncmp(ppdr.ref_coord, "EARTH FIXED REFERENCE SYSTEM",28))
areInertial = 0;
if (ppdr.hr_angle<=-99.0)
areInertial=0;/*Bogus GHA-- must be fixed-earth*/
if (ceos->facility==ASF && ceos->processor==SP2 && ceos->version <=2.50)
/*The SCANSAR processor made very odd state vectors before 2.51*/
areInertial=0,areInertialVelocity=1;
/*Fill output record with inital time.*/
if (ceos->facility==ASF && ceos->processor!=LZP)
{/* ASF's state vectors start at the
same time as the images themselves.*/
timeStart = 0.0;
s->year = (int) ppdr.year;
s->julDay = (int) ppdr.gmt_day;
s->second = ppdr.gmt_sec;
}
else
{/*Most facility's state vectors don't necessarily start
at the same time as the image.*/
timeStart=get_timeDelta(ceos,&ppdr,meta);
s->year = (int) ppdr.year;
s->julDay = (int) ppdr.gmt_day;
s->second = ppdr.gmt_sec;
}
/*Fill ouput record with state vectors.*/
for (i=0; i<s->vector_count; i++) {
/*Read a state vector from the record, fixing the units.*/
stateVector st;
st.pos.x = ppdr.pos_vec[i][0];
st.pos.y = ppdr.pos_vec[i][1];
st.pos.z = ppdr.pos_vec[i][2];
vecScale(&st.pos,get_units(vecMagnitude(st.pos),EXPECTED_POS));
st.vel.x = ppdr.pos_vec[i][3];
st.vel.y = ppdr.pos_vec[i][4];
st.vel.z = ppdr.pos_vec[i][5];
vecScale(&st.vel,get_units(vecMagnitude(st.vel),EXPECTED_VEL));
/*Correct for non-rotating frame.*/
if (areInertial)
gei2fixed(&st,ppdr2gha(&ppdr,ceos,i*ppdr.data_int));
/*Perform velocity fixes.*/
if (areInertialVelocity)
gei2fixed(&st,0.0);
else if (areFixedVelocity)
fixed2gei(&st,0.0);
/*Write out resulting state vectors.*/
s->vecs[i].vec = st;
s->vecs[i].time = timeStart+i*ppdr.data_int;
}
}
virtual void KeyCallBack(unsigned char key, int x, int y) {
bool needRedisplay = true;
switch (key) {
case 'p': {
// Write image to PPM file
std::ofstream f("image.ppm", std::ofstream::trunc);
if (!f.good()) {
OCLTOY_LOG("Failed to open image file: image.ppm");
} else {
f << "P3" << std::endl;
f << windowWidth << " " << windowHeight << std::endl;
f << "255" << std::endl;
for (int y = windowHeight - 1; y >= 0; --y) {
const PixelRGBA8888 *p = &bitmap->pixels[y * windowWidth];
for (int x = 0; x < windowWidth; ++x, p++) {
const std::string r = boost::lexical_cast<std::string>((unsigned int)p->r);
const std::string g = boost::lexical_cast<std::string>((unsigned int)p->g);
const std::string b = boost::lexical_cast<std::string>((unsigned int)p->b);
f << r << " " << g << " " << b << std::endl;
}
}
}
f.close();
OCLTOY_LOG("Saved framebuffer in image.ppm");
needRedisplay = false;
break;
}
case 27: // Escape key
case 'q':
case 'Q':
OCLTOY_LOG("Done");
exit(EXIT_SUCCESS);
break;
case ' ': // Restart rendering
animCamera = true;
sceneTimeOffset = WallClockTime();
setupAnim(scene, windowWidth, windowHeight);
break;
case 'h':
printHelp = (!printHelp);
break;
case 'a': {
animCamera = false;
cl_float3 dir = scene->cam.viewRight;
vecNormalize(dir);
vecScale(dir, -MOVE_STEP);
camMove(scene->cam, dir);
break;
}
case 'd': {
animCamera = false;
cl_float3 dir = scene->cam.viewRight;
vecNormalize(dir);
vecScale(dir, MOVE_STEP);
camMove(scene->cam, dir);
break;
}
case 'w': {
animCamera = false;
cl_float3 dir = scene->cam.viewCenter;
vecSub(dir, scene->cam.eye);
vecNormalize(dir);
vecScale(dir, MOVE_STEP);
camMove(scene->cam, dir);
break;
}
case 's': {
animCamera = false;
cl_float3 dir = scene->cam.viewCenter;
vecSub(dir, scene->cam.eye);
vecNormalize(dir);
vecScale(dir, -MOVE_STEP);
camMove(scene->cam, dir);
break;
}
case 'r': {
animCamera = false;
cl_float3 dir;
vecInit(dir, 0.f, MOVE_STEP, 0.f);
camMove(scene->cam, dir);
break;
}
case 'f': {
animCamera = false;
cl_float3 dir;
vecInit(dir, 0.f, -MOVE_STEP, 0.f);
camMove(scene->cam, dir);
break;
}
default:
needRedisplay = false;
break;
}
if (needRedisplay)
glutPostRedisplay();
}
