bool Cube::isHit( Ray &ray,
vect3d &pNormal, float *t, vect3d *pTexColor)
{
if(!_bbox.isHit(ray))
{
return false;
}
float fCurrT = 0xFFFFFFFF;
float t0 = 0xFFFFFFFF;
vect3d vCurrNormal;
for(int i = 0; i < 6; i ++)
{
t0 = 0xFFFFFFFF;
vect3d tmpv;
bool bHit = _vs[i]->isHit(ray, tmpv, &t0, pTexColor);
if( bHit && (t0 < fCurrT) )
{
fCurrT = t0;
vecCopy(vCurrNormal, tmpv);
}
}
if(fCurrT == 0xFFFFFFFF)
{
return false;
}
*t = fCurrT;
vecCopy(pNormal, vCurrNormal);
return true;
}
void Object::setMaterial(vect3d &specColor, vect3d &diffColor, vect3d &ambiColor, float fShininess)
{
vecCopy(_mat.specColor, specColor);
vecCopy(_mat.diffColor, diffColor);
vecCopy(_mat.ambiColor, ambiColor);
_mat.fShininess = fShininess;
}
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();
}
void extended_kalman_init( uFloat **P, uFloat *x )
{
#ifdef PRINT_DEBUG
printf( "ekf: Initializing filter\n" );
#endif
alloc_globals( STATE_SIZE, MEAS_SIZE );
sys_transfer = matrix( 1, STATE_SIZE, 1, STATE_SIZE );
mea_transfer = matrix( 1, MEAS_SIZE, 1, STATE_SIZE );
/* Init the global variables using the arguments. */
vecCopy( x, state_post, STATE_SIZE );
vecCopy( x, state_pre, STATE_SIZE );
matCopy( P, cov_post, STATE_SIZE, STATE_SIZE );
matCopy( P, cov_pre, STATE_SIZE, STATE_SIZE );
covarianceSet( sys_noise_cov, mea_noise_cov );
#ifdef PRINT_DEBUG
printf( "ekf: Completed Initialization\n" );
#endif
}
Platform::Platform(GameObject * source)
{
body.type = RECTANGLE;
id = PLATFORM;
tex_id = source->tex_id;
vecCopy(source->body.center, body.center);
vecCopy(source->prevPosition, prevPosition);
vecCopy(source->velocity, velocity);
body.width = source->body.width;
body.height = source->body.height;
textureWidth = source->textureWidth;
textureHeight = source->textureHeight;
horizontalTiles = source->horizontalTiles;
verticalTiles = source->verticalTiles;
for (int i = 0; i < 3; i++) {
rgb[i] = source->rgb[i];
}
}
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);
}
}
// Does the same as R function table
std::map<int, int> tabulateVector(std::vector<int>& vec) {
std::vector<int> vecCopy(vec);
std::sort(vecCopy.begin(), vecCopy.end());
std::vector<int>::iterator it = std::unique(vecCopy.begin(), vecCopy.end());
vecCopy.resize(std::distance(vecCopy.begin(), it));
std::map<int, int> table;
//int pos = 0;
for (std::vector<int>::size_type i = 0; i != vecCopy.size(); i++) {
int mycount = std::count(vec.begin(), vec.end(), vecCopy[i]);
table[vecCopy[i]] = mycount;
//pos = pos + mycount;
}
return table;
}
/* --------------- Platform Class --------------- */
Platform::Platform(int width, int height, int x, int y, int tex)
{
body.type = RECTANGLE;
id = PLATFORM;
tex_id = tex;
vecMake(x,y,body.center);
vecCopy(body.center, prevPosition);
vecZero(velocity);
body.width = width;
body.height = height;
textureWidth = 15; // !!!!! need to merge with game's texture data !!!!!
textureHeight = 15;
horizontalTiles = width / textureWidth;
verticalTiles = height / textureHeight;
rgb[0] = 90;
rgb[1] = 140;
rgb[2] = 90;
}
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;
}
static void rungeKutta( uFloat *old_state, uFloat *new_state )
{
#ifdef PRINT_DEBUG
printf( "ekf: rungeKutta\n" );
#endif
/* k1 = period * F( old_state ) */
systemF( tempState, old_state );
vecScalarMult( tempState, PERIOD, k1, STATE_SIZE );
/* k2 = period * F( old_state + k1 * 0.5) */
vecScalarMult( k1, 0.5, tempState, STATE_SIZE ); /* tempState = k1 * 0.5 */
vecAdd( old_state, tempState, tempState, STATE_SIZE ); /* tempState = old_state + k1 * 0.5 */
systemF( tempState, old_state );
vecScalarMult( tempState, PERIOD, k2, STATE_SIZE );
/* k3 = period * F( old_state + k2 * 0.5) */
vecScalarMult( k2, 0.5, tempState, STATE_SIZE ); /* tempState = k2 * 0.5 */
vecAdd( old_state, tempState, tempState, STATE_SIZE ); /* tempState = old_state + k2 * 0.5 */
systemF( tempState, old_state );
vecScalarMult( tempState, PERIOD, k3, STATE_SIZE );
/* k4 = period * F( old_state + k3) */
vecAdd( old_state, tempState, tempState, STATE_SIZE ); /* tempState = old_state + k3 */
systemF( tempState, old_state );
vecScalarMult( tempState, PERIOD, k4, STATE_SIZE );
/* new_state = old_state + 0.16667 * ( k1 + 2*k2 + 2*k3 + k4 ) */
vecCopy( k1, new_state, STATE_SIZE );
vecScalarMult( k2, 2.0, tempState, STATE_SIZE );
vecAdd( new_state, tempState, new_state, STATE_SIZE);
vecScalarMult( k3, 2.0, tempState, STATE_SIZE );
vecAdd( new_state, tempState, new_state, STATE_SIZE);
vecAdd( new_state, k4, new_state, STATE_SIZE);
vecScalarMult( new_state, 0.16667, new_state, STATE_SIZE );
vecAdd( new_state, old_state, new_state, STATE_SIZE);
}
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 cameraSetView(Camera * camera, float4 pos, float4 target, float4 up) {
vecCopy(camera->position, pos);
vecCopy(camera->target, target);
vecCopy(camera->up, up);
}