/**
* Culls OBB to n sided frustrum. Normals pointing outwards.
* -> IN: RebPlane - array of planes building frustrum
* int - number of planes in array
* OUT: RebVISIBLE - obb totally inside frustrum
* RebCLIPPED - obb clipped by frustrum
* RebCULLED - obb totally outside frustrum
*/
int RebObb::Cull(const RebPlane *pPlanes, int nNumPlanes) {
RebVector vN;
int nResult = RebVISIBLE;
float fRadius, fTest;
// for all planes
for (int i=0; i<nNumPlanes; i++) {
// frustrum normals pointing outwards, we need inwards
vN = pPlanes[i].m_vcN * -1.0f;
// calculate projected box radius
fRadius = _fabs(fA0 * (vN * vcA0))
+ _fabs(fA1 * (vN * vcA1))
+ _fabs(fA2 * (vN * vcA2));
// testvalue: (N*C - d) (#)
fTest = vN * this->vcCenter - pPlanes[i].m_fD;
// obb totally outside of at least one plane: (#) < -r
if (fTest < -fRadius)
return RebCULLED;
// or maybe intersecting this plane?
else if (!(fTest > fRadius))
nResult = RebCLIPPED;
} // for
// if not culled then clipped or inside
return nResult;
}
// Intersection with OBB. Same as obb culling to frustrum planes.
bool RebPlane::Intersects(const RebObb &obb) {
float fRadius = _fabs( obb.fA0 * (m_vcN * obb.vcA0) )
+ _fabs( obb.fA1 * (m_vcN * obb.vcA1) )
+ _fabs( obb.fA2 * (m_vcN * obb.vcA2) );
float fDistance = this->Distance(obb.vcCenter);
return (fDistance <= fRadius);
} // Intersects(OBB)
// test for intersection with aabb, original code by Andrew Woo,
// from "Geometric Tools...", Morgan Kaufmann Publ., 2002
bool ZFXRay::Intersects(const ZFXAabb &aabb, float fL, float *t) {
bool bInside = true;
float t0, t1, tmp, tFinal;
float tNear = -999999.9f;
float tFar = 999999.9f;
float epsilon = 0.00001f;
ZFXVector MaxT;
// first pair of planes
if (_fabs(m_vcDir.x) < epsilon) {
if ((m_vcOrig.x < aabb.vcMin.x) ||
(m_vcOrig.x > aabb.vcMax.x))
return false;
}
t0 = (aabb.vcMin.x - m_vcOrig.x) / m_vcDir.x;
t1 = (aabb.vcMax.x - m_vcOrig.x) / m_vcDir.x;
if (t0 > t1) { tmp = t0; t0 = t1; t1 = tmp; }
if (t0 > tNear) tNear = t0;
if (t1 < tFar) tFar = t1;
if (tNear > tFar) return false;
if (tFar < 0) return false;
// second pair of planes
if (_fabs(m_vcDir.y) < epsilon) {
if ((m_vcOrig.y < aabb.vcMin.y) ||
(m_vcOrig.y > aabb.vcMax.y))
return false;
}
t0 = (aabb.vcMin.y - m_vcOrig.y) / m_vcDir.y;
t1 = (aabb.vcMax.y - m_vcOrig.y) / m_vcDir.y;
if (t0 > t1) { tmp = t0; t0 = t1; t1 = tmp; }
if (t0 > tNear) tNear = t0;
if (t1 < tFar) tFar = t1;
if (tNear > tFar) return false;
if (tFar < 0) return false;
// third pair of planes
if (_fabs(m_vcDir.z) < epsilon) {
if ((m_vcOrig.z < aabb.vcMin.z) ||
(m_vcOrig.z > aabb.vcMax.z))
return false;
}
t0 = (aabb.vcMin.z - m_vcOrig.z) / m_vcDir.z;
t1 = (aabb.vcMax.z - m_vcOrig.z) / m_vcDir.z;
if (t0 > t1) { tmp = t0; t0 = t1; t1 = tmp; }
if (t0 > tNear) tNear = t0;
if (t1 < tFar) tFar = t1;
if (tNear > tFar) return false;
if (tFar < 0) return false;
if (tNear > 0) tFinal = tNear;
else tFinal = tFar;
if (tFinal > fL) return false;
if (t) *t = tFinal;
return true;
} // Intersects(Aabb) at length
// test for intersection with aabb, original code by Andrew Woo,
// from "Geometric Tools...", Morgan Kaufmann Publ., 2002
bool RebAabb::Intersects(const RebRay &Ray, float fL, float *t) {
bool bInside = true;
float t0, t1, tmp, tFinal;
float tNear = -999999.9f;
float tFar = 999999.9f;
float epsilon = 0.00001f;
RebVector MaxT;
// first pair of planes
if (_fabs(Ray.m_vcDir.x) < epsilon) {
if ( (Ray.m_vcOrig.x < vcMin.x) ||
(Ray.m_vcOrig.x > vcMax.x) )
return false;
}
t0 = (vcMin.x - Ray.m_vcOrig.x) / Ray.m_vcDir.x;
t1 = (vcMax.x - Ray.m_vcOrig.x) / Ray.m_vcDir.x;
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
if (t0 > tNear) tNear = t0;
if (t1 < tFar) tFar = t1;
if (tNear > tFar) return false;
if (tFar < 0) return false;
// second pair of planes
if (_fabs(Ray.m_vcDir.y) < epsilon) {
if ( (Ray.m_vcOrig.y < vcMin.y) ||
(Ray.m_vcOrig.y > vcMax.y) )
return false;
}
t0 = (vcMin.y - Ray.m_vcOrig.y) / Ray.m_vcDir.y;
t1 = (vcMax.y - Ray.m_vcOrig.y) / Ray.m_vcDir.y;
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
if (t0 > tNear) tNear = t0;
if (t1 < tFar) tFar = t1;
if (tNear > tFar) return false;
if (tFar < 0) return false;
// third pair of planes
if (_fabs(Ray.m_vcDir.z) < epsilon) {
if ( (Ray.m_vcOrig.z < vcMin.z) ||
(Ray.m_vcOrig.z > vcMax.z) )
return false;
}
t0 = (vcMin.z - Ray.m_vcOrig.z) / Ray.m_vcDir.z;
t1 = (vcMax.z - Ray.m_vcOrig.z) / Ray.m_vcDir.z;
if (t0 > t1) { tmp=t0; t0=t1; t1=tmp; }
if (t0 > tNear) tNear = t0;
if (t1 < tFar) tFar = t1;
if (tNear > tFar) return false;
if (tFar < 0) return false;
if (tNear > 0) tFinal = tNear;
else tFinal = tFar;
if (tFinal > fL) return false;
if (t) *t = tFinal;
return true;
} // Intersects(Ray) at length
// helperfunction
void RebObb::ObbProj(const RebObb &Obb, const RebVector &vcV,
float *pfMin, float *pfMax) {
float fDP = vcV * Obb.vcCenter;
float fR = Obb.fA0 * _fabs(vcV * Obb.vcA0) +
Obb.fA0 * _fabs(vcV * Obb.vcA1) +
Obb.fA1 * _fabs(vcV * Obb.vcA2);
*pfMin = fDP - fR;
*pfMax = fDP + fR;
} // ObbProj
// Intersection of two planes. If third parameter is given the line
// of intersection will be calculated. (www.magic-software.com)
bool RebPlane::Intersects(const RebPlane &plane, RebRay *pIntersection) {
RebVector vcCross;
float fSqrLength;
// if crossproduct of normals 0 than planes parallel
vcCross.Cross(this->m_vcN, plane.m_vcN);
fSqrLength = vcCross.GetSqrLength();
if (fSqrLength < 1e-08f)
return false;
// find line of intersection
if (pIntersection) {
float fN00 = this->m_vcN.GetSqrLength();
float fN01 = this->m_vcN * plane.m_vcN;
float fN11 = plane.m_vcN.GetSqrLength();
float fDet = fN00*fN11 - fN01*fN01;
if (_fabs(fDet) < 1e-08f)
return false;
float fInvDet = 1.0f/fDet;
float fC0 = (fN11*this->m_fD - fN01*plane.m_fD) * fInvDet;
float fC1 = (fN00*plane.m_fD - fN01*this->m_fD) * fInvDet;
(*pIntersection).m_vcDir = vcCross;
(*pIntersection).m_vcOrig = this->m_vcN*fC0 + plane.m_vcN*fC1;
}
return true;
} // Intersects(Plane)
// Intersection with Plane at distance fL.
bool ZFXRay::Intersects(const ZFXPlane &plane, bool bCull, float fL,
float *t, ZFXVector *vcHit) {
float Vd = plane.m_vcN * m_vcDir;
// ray parallel to plane
if (_fabs(Vd) < 0.00001f)
return false;
// normal pointing away from ray dir
// => intersection backface if any
if (bCull && (Vd > 0.0f))
return false;
float Vo = -((plane.m_vcN * m_vcOrig) + plane.m_fD);
float _t = Vo / Vd;
// intersection behind ray origin or beyond valid range
if ((_t < 0.0f) || (_t > fL))
return false;
if (vcHit) {
(*vcHit) = m_vcOrig + (m_vcDir * _t);
}
if (t)
(*t) = _t;
return true;
} // Intersects(Plane)
bool ZFXRay::Intersects(const ZFXPlane &plane, bool bCull, float fL, float *t, ZFXVector *vcHit){
float Vd=plane.m_vcN* m_vcDir;
//ray parallel to the plane
if(_fabs(Vd)<0.00001f)
return false;
//Plane normal point away from ray direction
//intersection with back face if any
if (bCull && (Vd>0.0f))
return false;
float Vo= -((plane.m_vcN*m_vcOrig)+plane.m_fD);
float _t=Vo/Vd;
//Intersection before ray origin
if(_t<0.0f || _t>fL)
return false;
if (vcHit){
(*vcHit)=m_vcOrig+(m_vcDir*_t);
}
if(t)
(*t)=_t;
return true;
}
// Calculate distance to point. Plane normal must be normalized.
float RebPlane::Distance(const RebVector &vcPoint) {
return ( _fabs((m_vcN*vcPoint) - m_fD) );
}
// test for intersection with obb at certain length (line segment),
// slaps method but compare result if true to length prior return.
bool ZFXRay::Intersects(const ZFXObb &obb, float fL, float *t) {
float e, f, t1, t2, temp;
float tmin = -99999.9f,
tmax = +99999.9f;
ZFXVector vcP = obb.vcCenter - m_vcOrig;
// 1st slap
e = obb.vcA0 * vcP;
f = obb.vcA0 * m_vcDir;
if (_fabs(f) > 0.00001f) {
t1 = (e + obb.fA0) / f;
t2 = (e - obb.fA0) / f;
if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (tmax < 0.0f) return false;
}
else if (((-e - obb.fA0) > 0.0f) || ((-e + obb.fA0) < 0.0f))
return false;
// 2nd slap
e = obb.vcA1 * vcP;
f = obb.vcA1 * m_vcDir;
if (_fabs(f) > 0.00001f) {
t1 = (e + obb.fA1) / f;
t2 = (e - obb.fA1) / f;
if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (tmax < 0.0f) return false;
}
else if (((-e - obb.fA1) > 0.0f) || ((-e + obb.fA1) < 0.0f))
return false;
// 3rd slap
e = obb.vcA2 * vcP;
f = obb.vcA2 * m_vcDir;
if (_fabs(f) > 0.00001f) {
t1 = (e + obb.fA2) / f;
t2 = (e - obb.fA2) / f;
if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (tmax < 0.0f) return false;
}
else if (((-e - obb.fA2) > 0.0f) || ((-e + obb.fA2) < 0.0f))
return false;
if ((tmin > 0.0f) && (tmin <= fL)) {
if (t) *t = tmin;
return true;
}
// intersection on line but not on segment
if (tmax > fL) return false;
if (t) *t = tmax;
return true;
} // Intersects(Obb at length)
// intersects another obb
bool RebObb::Intersects(const RebObb &Obb) {
float T[3];
// difference vector between both obb
RebVector vcD = Obb.vcCenter - this->vcCenter;
float matM[3][3]; // B's axis in relation to A
float ra, // radius A
rb, // radius B
t; // absolute values from T[]
// Obb A's axis as separation axis?
// ================================
// first axis vcA0
matM[0][0] = this->vcA0 * Obb.vcA0;
matM[0][1] = this->vcA0 * Obb.vcA1;
matM[0][2] = this->vcA0 * Obb.vcA2;
ra = this->fA0;
rb = Obb.fA0 * _fabs(matM[0][0]) +
Obb.fA1 * _fabs(matM[0][1]) +
Obb.fA2 * _fabs(matM[0][2]);
T[0] = vcD * this->vcA0;
t = _fabs(T[0]);
if(t > (ra + rb) )
return false;
// second axis vcA1
matM[1][0] = this->vcA1 * Obb.vcA0;
matM[1][1] = this->vcA1 * Obb.vcA1;
matM[1][2] = this->vcA1 * Obb.vcA2;
ra = this->fA1;
rb = Obb.fA0 * _fabs(matM[1][0]) +
Obb.fA1 * _fabs(matM[1][1]) +
Obb.fA2 * _fabs(matM[1][2]);
T[1] = vcD * this->vcA1;
t = _fabs(T[1]);
if(t > (ra + rb) )
return false;
// third axis vcA2
matM[2][0] = this->vcA2 * Obb.vcA0;
matM[2][1] = this->vcA2 * Obb.vcA1;
matM[2][2] = this->vcA2 * Obb.vcA2;
ra = this->fA2;
rb = Obb.fA0 * _fabs(matM[2][0]) +
Obb.fA1 * _fabs(matM[2][1]) +
Obb.fA2 * _fabs(matM[2][2]);
T[2] = vcD * this->vcA2;
t = _fabs(T[2]);
if(t > (ra + rb) )
return false;
// Obb B's axis as separation axis?
// ================================
// first axis vcA0
ra = this->fA0 * _fabs(matM[0][0]) +
this->fA1 * _fabs(matM[1][0]) +
this->fA2 * _fabs(matM[2][0]);
rb = Obb.fA0;
t = _fabs( T[0]*matM[0][0] + T[1]*matM[1][0] + T[2]*matM[2][0] );
if(t > (ra + rb) )
return false;
// second axis vcA1
ra = this->fA0 * _fabs(matM[0][1]) +
this->fA1 * _fabs(matM[1][1]) +
this->fA2 * _fabs(matM[2][1]);
rb = Obb.fA1;
t = _fabs( T[0]*matM[0][1] + T[1]*matM[1][1] + T[2]*matM[2][1] );
if(t > (ra + rb) )
return false;
// third axis vcA2
ra = this->fA0 * _fabs(matM[0][2]) +
this->fA1 * _fabs(matM[1][2]) +
this->fA2 * _fabs(matM[2][2]);
rb = Obb.fA2;
t = _fabs( T[0]*matM[0][2] + T[1]*matM[1][2] + T[2]*matM[2][2] );
if(t > (ra + rb) )
return false;
// other candidates: cross products of axis:
// =========================================
// axis A0xB0
ra = this->fA1*_fabs(matM[2][0]) + this->fA2*_fabs(matM[1][0]);
rb = Obb.fA1*_fabs(matM[0][2]) + Obb.fA2*_fabs(matM[0][1]);
t = _fabs( T[2]*matM[1][0] - T[1]*matM[2][0] );
if( t > ra + rb )
return false;
// axis A0xB1
ra = this->fA1*_fabs(matM[2][1]) + this->fA2*_fabs(matM[1][1]);
rb = Obb.fA0*_fabs(matM[0][2]) + Obb.fA2*_fabs(matM[0][0]);
t = _fabs( T[2]*matM[1][1] - T[1]*matM[2][1] );
if( t > ra + rb )
return false;
// axis A0xB2
ra = this->fA1*_fabs(matM[2][2]) + this->fA2*_fabs(matM[1][2]);
rb = Obb.fA0*_fabs(matM[0][1]) + Obb.fA1*_fabs(matM[0][0]);
t = _fabs( T[2]*matM[1][2] - T[1]*matM[2][2] );
if( t > ra + rb )
return false;
// axis A1xB0
ra = this->fA0*_fabs(matM[2][0]) + this->fA2*_fabs(matM[0][0]);
rb = Obb.fA1*_fabs(matM[1][2]) + Obb.fA2*_fabs(matM[1][1]);
t = _fabs( T[0]*matM[2][0] - T[2]*matM[0][0] );
if( t > ra + rb )
return false;
// axis A1xB1
ra = this->fA0*_fabs(matM[2][1]) + this->fA2*_fabs(matM[0][1]);
rb = Obb.fA0*_fabs(matM[1][2]) + Obb.fA2*_fabs(matM[1][0]);
t = _fabs( T[0]*matM[2][1] - T[2]*matM[0][1] );
if( t > ra + rb )
return false;
// axis A1xB2
ra = this->fA0*_fabs(matM[2][2]) + this->fA2*_fabs(matM[0][2]);
rb = Obb.fA0*_fabs(matM[1][1]) + Obb.fA1*_fabs(matM[1][0]);
t = _fabs( T[0]*matM[2][2] - T[2]*matM[0][2] );
if( t > ra + rb )
return false;
// axis A2xB0
ra = this->fA0*_fabs(matM[1][0]) + this->fA1*_fabs(matM[0][0]);
rb = Obb.fA1*_fabs(matM[2][2]) + Obb.fA2*_fabs(matM[2][1]);
t = _fabs( T[1]*matM[0][0] - T[0]*matM[1][0] );
if( t > ra + rb )
return false;
// axis A2xB1
ra = this->fA0*_fabs(matM[1][1]) + this->fA1*_fabs(matM[0][1]);
rb = Obb.fA0 *_fabs(matM[2][2]) + Obb.fA2*_fabs(matM[2][0]);
t = _fabs( T[1]*matM[0][1] - T[0]*matM[1][1] );
if( t > ra + rb )
return false;
// axis A2xB2
ra = this->fA0*_fabs(matM[1][2]) + this->fA1*_fabs(matM[0][2]);
rb = Obb.fA0*_fabs(matM[2][1]) + Obb.fA1*_fabs(matM[2][0]);
t = _fabs( T[1]*matM[0][2] - T[0]*matM[1][2] );
if( t > ra + rb )
return false;
// no separation axis found => intersection
return true;
} // Intersects(obb)
// intersects ray at certain length (line segment), slaps method
bool RebObb::Intersects(const RebRay &Ray, float fL, float *t) {
float e, f, t1, t2, temp;
float tmin = -99999.9f,
tmax = +99999.9f;
RebVector vcP = this->vcCenter - Ray.m_vcOrig;
// 1st slap
e = this->vcA0 * vcP;
f = this->vcA0 * Ray.m_vcDir;
if (_fabs(f) > 0.00001f) {
t1 = (e + this->fA0) / f;
t2 = (e - this->fA0) / f;
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (tmax < 0.0f) return false;
}
else if ( ((-e - this->fA0) > 0.0f) || ((-e + this->fA0) < 0.0f) )
return false;
// 2nd slap
e = this->vcA1 * vcP;
f = this->vcA1 * Ray.m_vcDir;
if (_fabs(f) > 0.00001f) {
t1 = (e + this->fA1) / f;
t2 = (e - this->fA1) / f;
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (tmax < 0.0f) return false;
}
else if ( ((-e - this->fA1) > 0.0f) || ((-e + this->fA1) < 0.0f) )
return false;
// 3rd slap
e = this->vcA2 * vcP;
f = this->vcA2 * Ray.m_vcDir;
if (_fabs(f) > 0.00001f) {
t1 = (e + this->fA2) / f;
t2 = (e - this->fA2) / f;
if (t1 > t2) { temp=t1; t1=t2; t2=temp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (tmax < 0.0f) return false;
}
else if ( ((-e - this->fA2) > 0.0f) || ((-e + this->fA2) < 0.0f) )
return false;
if ( (tmin > 0.0f) && (tmin <= fL) ) {
if (t) *t = tmin;
return true;
}
// intersection on line but not on segment
if (tmax > fL) return false;
if (t) *t = tmax;
return true;
} // Intersects(Segment)
/**
* convex quad-box overlap test, based on the separating
* axis therom. see Moller, JGT 6(1), 2001 for
* derivation of the tri case.
*
* \param quad
* \param xmin
* \param ymin
* \param xmax
* \param ymax
*
* \return 0 ==> no overlap
* 1 ==> overlap
*/
static int
quad_overlap(float *quad, float xmin, float ymin, float xmax, float ymax)
{
int a;
float v[4][2], f[4][2], c[2], h[2];
float p0, p1, p2, r;
/* find the center */
c[0] = (xmin + xmax) * (GLfloat).5;
c[1] = (ymin + ymax) * (GLfloat).5;
h[0] = xmax - c[0];
h[1] = ymax - c[1];
/* translate everything to be at the origin */
v[0][0] = quad[0] - c[0];
v[0][1] = quad[1] - c[1];
v[1][0] = quad[2] - c[0];
v[1][1] = quad[3] - c[1];
v[2][0] = quad[4] - c[0];
v[2][1] = quad[5] - c[1];
v[3][0] = quad[6] - c[0];
v[3][1] = quad[7] - c[1];
/* XXX: \todo this should be pre-computed */
for (a=0; a<2; a++)
{
f[0][a] = v[1][a] - v[0][a];
f[1][a] = v[2][a] - v[1][a];
f[2][a] = v[3][a] - v[2][a];
f[3][a] = v[0][a] - v[3][a];
}
/* now, test the x & y axes (e0 & e1) */
if ((_max2f(_max2f(v[0][0], v[1][0]), _max2f(v[2][0], v[3][0])) < -h[0]) ||
(_min2f(_min2f(v[0][0], v[1][0]), _min2f(v[2][0], v[3][0])) > h[0]))
return 0;
if ((_max2f(_max2f(v[0][1], v[1][1]), _max2f(v[2][1], v[3][1])) < -h[1]) ||
(_min2f(_min2f(v[0][1], v[1][1]), _min2f(v[2][1], v[3][1])) > h[1]))
return 0;
/* a0* and a1* reduce to e2, so no bother */
/* a20 = (-f0y, f0x, 0) */
p0 = v[1][0]*v[0][1] - v[0][0]*v[1][1];
p1 = v[2][1]*f[0][0] - v[2][0]*f[0][1];
p2 = v[3][1]*f[0][0] - v[3][0]*f[0][1];
r = h[0]*_fabs(f[0][1]) + h[1]*_fabs(f[0][0]);
if ((_min3f(p0, p1, p2) > r) || (_max3f(p0, p1, p2) < -r)) return 0;
/* a21 = (-f1y, f1x, 0) */
p0 = v[2][0]*v[1][1] - v[1][0]*v[2][1];
p1 = v[0][1]*f[1][0] - v[0][0]*f[1][1];
p2 = v[3][1]*f[1][0] - v[3][0]*f[1][1];
r = h[0]*_fabs(f[1][1]) + h[1]*_fabs(f[1][0]);
if ((_min3f(p0, p1, p2) > r) || (_max3f(p0, p1, p2) < -r)) return 0;
/* a22 = (-f2y, f2x, 0) */
p0 = v[3][0]*v[2][1] - v[2][0]*v[3][1];
p1 = v[0][1]*f[2][0] - v[0][0]*f[2][1];
p2 = v[1][1]*f[2][0] - v[1][0]*f[2][1];
r = h[0]*_fabs(f[2][1]) + h[1]*_fabs(f[2][0]);
if ((_min3f(p0, p1, p2) > r) || (_max3f(p0, p1, p2) < -r)) return 0;
/* a23 = (-f3y, f3x, 0) */
p0 = v[0][0]*v[3][1] - v[3][0]*v[0][1];
p1 = v[1][1]*f[3][0] - v[1][0]*f[3][1];
p2 = v[2][1]*f[3][0] - v[2][0]*f[3][1];
r = h[0]*_fabs(f[3][1]) + h[1]*_fabs(f[3][0]);
if ((_min3f(p0, p1, p2) > r) || (_max3f(p0, p1, p2) < -r)) return 0;
return 1;
}
double fabs(double x)
{
return _fabs(x);
}
bool ZFXRay::Intersects(const ZFXObb &pObb,float fL,float *t) const{
float e, f, t1,t2,temp;
float tmin= FLT_MIN;
float tmax=FLT_MAX;
ZFXVector vcP=pObb.vcCenter - m_vcOrig;
//1 Slap
e=pObb.vcA0 * vcP;
f=pObb.vcA0 * m_vcDir;
if(_fabs(f)>0.00001f){
t1=(e + pObb.fA0) / f;
t2=(e - pObb.fA0) / f;
if(t1>t2) { temp=t1;t1=t2;t2=temp;}
if(t1>tmin)tmin=t1;
if(t2<tmax)tmax=t2;
if(tmin>tmax) return false;
if(tmax<0.0f) return false;
}
else if (((-e - pObb.fA0) >0.0f) ||
((-e + pObb.fA0) <0.0f))
return false;
//2 Slap
e=pObb.vcA1 * vcP;
f=pObb.vcA1 * m_vcDir;
if(_fabs(f)>0.00001f){
t1=(e + pObb.fA1) / f;
t2=(e - pObb.fA1) / f;
if(t1>t2) { temp=t1;t1=t2;t2=temp;}
if(t1>tmin)tmin=t1;
if(t2<tmax)tmax=t2;
if(tmin>tmax) return false;
if(tmax<0.0f) return false;
}
else if (((-e - pObb.fA1) >0.0f) ||
((-e + pObb.fA1) <0.0f))
return false;
//3 Slap
e=pObb.vcA2 * vcP;
f=pObb.vcA2 * m_vcDir;
if(_fabs(f)>0.00001f){
t1=(e + pObb.fA2) / f;
t2=(e - pObb.fA2) / f;
if(t1>t2) { temp=t1;t1=t2;t2=temp;}
if(t1>tmin)tmin=t1;
if(t2<tmax)tmax=t2;
if(tmin>tmax) return false;
if(tmax<0.0f) return false;
}
else if (((-e - pObb.fA2) >0.0f) ||
((-e + pObb.fA2) <0.0f))
return false;
if(tmin>0.0f && (tmin <= fL)){
if (t) *t =tmin;
return true;
}
if (t) *t=tmax;
return true;
}
void test_Vectord(void)
{
LOG("* [Empty Vector]\n");
mcon::Vectord dvec;
// Zero length
CHECK_VALUE(dvec.GetLength(), 0);
// IsNull() is true.
CHECK_VALUE(dvec.IsNull(), true);
LOG("* [Resize]\n");
const int length = 6;
dvec.Resize(length);
CHECK_VALUE(dvec.GetLength(), length);
// IsNull() is false.
CHECK_VALUE(dvec.IsNull(), false);
for (size_t i = 0; i < length; ++i)
{
dvec[i] = i;
}
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], i);
}
LOG("* [operator+=(double)]\n");
// operator+=(T)
dvec += 1;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], i+1);
}
// operator*=(T)
LOG("* [operator*=(double)]\n");
dvec *= 10;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], (i+1)*10);
}
// operator/=(T)
LOG("* [operator/=(double)]\n");
dvec /= 5;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], (i+1)*2);
}
// operator-=(T)
LOG("* [operator-=(double)]\n");
dvec -= 5;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], (static_cast<int>(i)+1)*2-5);
}
LOG("* [Copy]\n");
mcon::Vectord dvec2(length*2);
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
dvec2[i] = -(i+1);
}
// Copy
dvec2.Copy(dvec);
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
if (0 <= i && i < length)
{
CHECK_VALUE(dvec2[i], (static_cast<int>(i)+1)*2-5);
}
}
LOG("* [operator=]\n");
// Substitution
dvec2 = dvec;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec2[i], (static_cast<int>(i)+1)*2-5);
}
dvec = 10;
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
dvec2[i] = i + 1;
}
LOG("* [operator?=]\n");
dvec += dvec2;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], (i+1) + 10);
}
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
dvec2[i] = (i & 1) ? 1.0 : 2.0;
}
dvec *= dvec2;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], ((i+1) + 10) * ((i & 1) ? 1.0 : 2.0));
}
dvec /= dvec2;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], (i+1) + 10);
}
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
dvec2[i] = i + 1;
}
dvec -= dvec2;
for (size_t i = 0; i < length; ++i)
{
CHECK_VALUE(dvec[i], 10);
}
LOG("* [Carveout]\n");
for (size_t i = 0; i < dvec.GetLength(); ++i)
{
dvec[i] = i + 1;
}
// dvec = {1, 2, 3, 4, 5, 6};
dvec2 = dvec(0, 0);
CHECK_VALUE(dvec2.IsNull(), true);
dvec2 = dvec(-1, 1);
CHECK_VALUE(dvec2.IsNull(), true);
dvec2 = dvec(dvec.GetLength(), 1);
CHECK_VALUE(dvec2.IsNull(), true);
dvec2 = dvec(dvec.GetLength()-1, 5);
CHECK_VALUE(dvec2.IsNull(), false);
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
CHECK_VALUE(dvec2[i], 6);
}
dvec2 = dvec(1, 3);
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
CHECK_VALUE(dvec2[i], i+2);
}
// Fifo
LOG("* [Fifo]\n");
double v = dvec2.Fifo(5);
CHECK_VALUE(v, 2);
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
CHECK_VALUE(dvec2[i], i+3);
}
// Unshift
LOG("* [Unshift]\n");
v = dvec2.Unshift(2);
CHECK_VALUE(v, 5);
for (size_t i = 0; i < dvec2.GetLength(); ++i)
{
CHECK_VALUE(dvec2[i], i+2);
}
// Maximum/Minimum
LOG("* [GetMaximum]\n");
{
const int lens[] = {1, 2, 3, 4, 5, 7, 8, 9, 31, 32, 33, 63, 64, 65};
for ( unsigned int i = 0; i < sizeof(lens)/sizeof(int); ++i )
{
double ans = - __DBL_MAX__;
const int n = lens[i];
LOG(" Lenght=%d:\n", n);
mcon::Vectord v(n);
// 昇順
for ( int k = 0; k < n; ++k )
{
v[k] = k - n/2;
if ( ans < v[k] )
{
ans = v[k];
}
}
CHECK_VALUE(ans, v.GetMaximum());
// 降順
ans = - __DBL_MAX__;
for ( int k = 0; k < n; ++k )
{
v[k] = n/2 - k;
if ( ans < v[k] )
{
ans = v[k];
}
}
CHECK_VALUE(ans, v.GetMaximum());
}
}
LOG("* [GetMinimum]\n");
{
const int lens[] = {1, 2, 3, 4, 5, 7, 8, 9, 31, 32, 33, 63, 64, 65};
for ( unsigned int i = 0; i < sizeof(lens)/sizeof(int); ++i )
{
double ans = __DBL_MAX__;
const int n = lens[i];
LOG(" Lenght=%d:\n", n);
mcon::Vectord v(n);
// 昇順
for ( int k = 0; k < n; ++k )
{
v[k] = k - n/2;
if ( ans > v[k] )
{
ans = v[k];
}
}
CHECK_VALUE(ans, v.GetMinimum());
// 降順
ans = __DBL_MAX__;
for ( int k = 0; k < n; ++k )
{
v[k] = n/2 - k;
if ( ans > v[k] )
{
ans = v[k];
}
}
CHECK_VALUE(ans, v.GetMinimum());
}
}
LOG("* [GetMaximumAbsolute]\n");
{
const int lens[] = {1, 2, 3, 4, 5, 7, 8, 9, 31, 32, 33, 63, 64, 65};
for ( unsigned int i = 0; i < sizeof(lens)/sizeof(int); ++i )
{
double ans = 0;
const int n = lens[i];
LOG(" Lenght=%d:\n", n);
mcon::Vectord v(n);
// 昇順
for ( int k = 0; k < n; ++k )
{
v[k] = (k + 1) * (k & 1 ? -1 : 1);
if ( ans < _fabs(v[k]) )
{
ans = _fabs(v[k]);
}
}
CHECK_VALUE(ans, v.GetMaximumAbsolute());
// 昇順 (符号が逆)
// 結果は変わらない
v *= -1;
CHECK_VALUE(ans, v.GetMaximumAbsolute());
// 降順
for ( int k = 0; k < n; ++k )
{
v[k] = n - k;
if ( ans < _fabs(v[k]) )
{
ans = _fabs(v[k]);
}
}
CHECK_VALUE(ans, v.GetMaximumAbsolute());
// 降順 (符号が逆)
// 結果は変わらない
v *= -1;
CHECK_VALUE(ans, v.GetMaximumAbsolute());
}
}
LOG("* [GetMinimumAbsolute]\n");
{
const int lens[] = {1, 2, 3, 4, 5, 7, 8, 9, 31, 32, 33, 63, 64, 65};
for ( unsigned int i = 0; i < sizeof(lens)/sizeof(int); ++i )
{
double ans = __DBL_MAX__;
const int n = lens[i];
LOG(" Lenght=%d:\n", n);
mcon::Vectord v(n);
// 昇順
for ( int k = 0; k < n; ++k )
{
v[k] = (k + 1) * (k & 1 ? -1 : 1);
if ( ans > _fabs(v[k]) )
{
ans = _fabs(v[k]);
}
}
CHECK_VALUE(ans, v.GetMinimumAbsolute());
// 昇順 (符号が逆)
// 結果は変わらない
v *= -1;
CHECK_VALUE(ans, v.GetMinimumAbsolute());
// 降順
ans = __DBL_MAX__;
for ( int k = 0; k < n; ++k )
{
v[k] = n - k;
if ( ans > _fabs(v[k]) )
{
ans = _fabs(v[k]);
}
}
CHECK_VALUE(ans, v.GetMinimumAbsolute());
// 降順 (符号が逆)
// 結果は変わらない
v *= -1;
CHECK_VALUE(ans, v.GetMinimumAbsolute());
}
}
LOG("* [GetSum/Average/GetNorm]\n");
{
const int lens[] = {1, 2, 3, 4, 5, 7, 8, 9, 31, 32, 33, 63, 64, 65};
for ( unsigned int i = 0; i < sizeof(lens)/sizeof(int); ++i )
{
const int n = lens[i];
LOG(" Lenght=%d:\n", n);
mcon::Vectord v(n);
double sum = 0;
double norm = 0;
for ( int k = 0; k < n; ++k )
{
v[k] = k + 1 - n/4;
sum += v[k];
norm += v[k]*v[k];
}
double ave = sum / n;
norm = sqrt(norm);
CHECK_VALUE(sum , v.GetSum());
CHECK_VALUE(ave , v.GetAverage());
CHECK_VALUE(norm, v.GetNorm());
}
}
LOG("* [Dot]\n");
{
const int lens[] = {1, 2, 3, 4, 5, 7, 8, 9, 31, 32, 33, 63, 64, 65};
for ( unsigned int i = 0; i < sizeof(lens)/sizeof(int); ++i )
{
const int n = lens[i];
LOG(" Lenght=%d:\n", n);
mcon::Vectord v(n);
mcon::Vectord w(n);
double dot = 0;
for ( int k = 0; k < n; ++k )
{
v[k] = k + 1 - n/4;
w[k] = 3*n/4 - k - 1;
dot += v[k]*w[k];
}
CHECK_VALUE(dot, v.GetDotProduct(w));
}
}
LOG("* [ToMatrix]\n");
for (size_t i = 0; i < dvec.GetLength(); ++i)
{
dvec[i] = i + 1;
}
const mcon::Matrix<double> m = dvec.ToMatrix();
CHECK_VALUE(m.GetRowLength(), 1);
CHECK_VALUE(m.GetColumnLength(), dvec.GetLength());
for (size_t i = 0; i < dvec.GetLength(); ++i)
{
CHECK_VALUE(m[0][i], i+1);
}
const mcon::Matrix<double> mt = dvec.ToMatrix().Transpose();
CHECK_VALUE(mt.GetRowLength(), dvec.GetLength());
CHECK_VALUE(mt.GetColumnLength(), 1);
for (size_t i = 0; i < dvec.GetLength(); ++i)
{
CHECK_VALUE(mt[i][0], i+1);
}
LOG("END\n");
}
int encode_op(char *opcode, char *op_data)
{
int rd,rs,rt,imm,funct,shaft,target;
char tmp[256];
const char *fi = "%s %d";
const char *fg = "%s %%g%d";
const char *ff = "%s %%f%d";
const char *fl = "%s %s";
const char *fgi = "%s %%g%d, %d";
const char *fgl = "%s %%g%d, %s";
const char *fgg = "%s %%g%d, %%g%d";
const char *fggl = "%s %%g%d, %%g%d, %s";
const char *fggi = "%s %%g%d, %%g%d, %d";
const char *fggg = "%s %%g%d, %%g%d, %%g%d";
const char *fff = "%s %%f%d, %%f%d";
const char *fgf = "%s %%g%d, %%f%d";
const char *ffg = "%s %%f%d, %%g%d";
const char *fffl = "%s %%f%d, %%f%d, %s";
const char *ffff = "%s %%f%d, %%f%d, %%f%d";
const char *ffgi = "%s %%f%d, %%g%d, %d";
const char *ffgg = "%s %%f%d, %%g%d, %%g%d";
char lname[256];
shaft = funct = target = 0;
if(strcmp(opcode, "mvhi") == 0){
if(sscanf(op_data, fgi, tmp, &rs, &imm) == 3)
return mvhi(rs,0,imm);
}
if(strcmp(opcode, "mvlo") == 0){
if(sscanf(op_data, fgi, tmp, &rs, &imm) == 3)
return mvlo(rs,0,imm);
}
if(strcmp(opcode, "add") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return add(rs,rt,rd,0);
}
if(strcmp(opcode, "nor") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return nor(rs,rt,rd,0);
}
if(strcmp(opcode, "sub") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return sub(rs,rt,rd,0);
}
if(strcmp(opcode, "mul") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return mul(rs,rt,rd,0);
}
if(strcmp(opcode, "addi") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return addi(rs,rt,imm);
}
if(strcmp(opcode, "subi") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return subi(rs,rt,imm);
}
if(strcmp(opcode, "muli") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return muli(rs,rt,imm);
}
if(strcmp(opcode, "input") == 0){
if(sscanf(op_data, fg, tmp, &rd) == 2)
return input(0,0,rd,0);
}
if(strcmp(opcode, "inputw") == 0){
if(sscanf(op_data, fg, tmp, &rd) == 2)
return inputw(0,0,rd,0);
}
if(strcmp(opcode, "inputf") == 0){
if(sscanf(op_data, ff, tmp, &rd) == 2)
return inputf(0,0,rd,0);
}
if(strcmp(opcode, "output") == 0){
if(sscanf(op_data, fg, tmp, &rs) == 2)
return output(rs,0,0,0);
}
if(strcmp(opcode, "outputw") == 0){
if(sscanf(op_data, fg, tmp, &rs) == 2)
return outputw(rs,0,0,0);
}
if(strcmp(opcode, "outputf") == 0){
if(sscanf(op_data, ff, tmp, &rs) == 2)
return outputf(rs,0,0,0);
}
if(strcmp(opcode, "and") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return _and(rs,rt,rd,0);
}
if(strcmp(opcode, "or") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return _or(rs,rt,rd,0);
}
if(strcmp(opcode, "sll") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return sll(rs,rt,rd,0);
}
if(strcmp(opcode, "srl") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return srl(rs,rt,rd,0);
}
if(strcmp(opcode, "slli") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return slli(rs,rt,imm);
}
if(strcmp(opcode, "srli") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return srli(rs,rt,imm);
}
if(strcmp(opcode, "b") == 0){
if(sscanf(op_data, fg, tmp, &rs) == 2)
return b(rs,0,0,0);
}
if(strcmp(opcode, "jmp") == 0){
if(sscanf(op_data, fl, tmp, lname) == 2) {
strcpy(label_name[label_cnt],lname);
return jmp(label_cnt++);
}
}
if(strcmp(opcode, "jeq") == 0){
if(sscanf(op_data, fggl, tmp, &rs, &rt, lname) == 4) {
strcpy(label_name[label_cnt],lname);
return jeq(rs,rt,label_cnt++);
}
}
if(strcmp(opcode, "jne") == 0){
if(sscanf(op_data, fggl, tmp, &rs, &rt, lname) == 4) {
strcpy(label_name[label_cnt],lname);
return jne(rs,rt,label_cnt++);
}
}
if(strcmp(opcode, "jlt") == 0){
if(sscanf(op_data, fggl, tmp, &rs, &rt, lname) == 4) {
strcpy(label_name[label_cnt],lname);
return jlt(rs,rt,label_cnt++);
}
}
if(strcmp(opcode, "jle") == 0){
if(sscanf(op_data, fggl, tmp, &rs, &rt, lname) == 4) {
strcpy(label_name[label_cnt],lname);
return jle(rs,rt,label_cnt++);
}
}
if(strcmp(opcode, "call") == 0){
if(sscanf(op_data, fl, tmp, lname) == 2) {
strcpy(label_name[label_cnt],lname);
return call(label_cnt++);
}
}
if(strcmp(opcode, "callR") == 0){
if(sscanf(op_data, fg, tmp, &rs) == 2)
return callr(rs,0,0,0);
}
if(strcmp(opcode, "return") == 0){
return _return(0);
}
if(strcmp(opcode, "ld") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return ld(rs,rt,rd,0);
}
if(strcmp(opcode, "ldi") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return ldi(rs,rt,imm);
}
if(strcmp(opcode, "ldlr") == 0){
if(sscanf(op_data, fgi, tmp, &rs, &imm) == 3)
return ldlr(rs,0,imm);
}
if(strcmp(opcode, "fld") == 0){
if(sscanf(op_data, ffgg, tmp, &rd, &rs,&rt) == 4)
return fld(rs,rt,rd,0);
}
if(strcmp(opcode, "st") == 0){
if(sscanf(op_data, fggg, tmp, &rd, &rs,&rt) == 4)
return st(rs,rt,rd,0);
}
if(strcmp(opcode, "sti") == 0){
if(sscanf(op_data, fggi, tmp, &rt, &rs, &imm) == 4)
return sti(rs,rt,imm);
}
if(strcmp(opcode, "stlr") == 0){
if(sscanf(op_data, fgi, tmp, &rs, &imm) == 3)
return stlr(rs,0,imm);
}
if(strcmp(opcode, "fst") == 0){
if(sscanf(op_data, ffgg, tmp, &rd, &rs,&rt) == 4)
return fst(rs,rt,rd,0);
}
if(strcmp(opcode, "fadd") == 0){
if(sscanf(op_data, ffff, tmp, &rd, &rs, &rt) == 4)
return fadd(rs,rt,rd,0);
}
if(strcmp(opcode, "fsub") == 0){
if(sscanf(op_data, ffff, tmp, &rd, &rs, &rt) == 4)
return fsub(rs,rt,rd,0);
}
if(strcmp(opcode, "fmul") == 0){
if(sscanf(op_data, ffff, tmp, &rd, &rs, &rt) == 4)
return fmul(rs,rt,rd,0);
}
if(strcmp(opcode, "fdiv") == 0){
if(sscanf(op_data, ffff, tmp, &rd, &rs, &rt) == 4)
return fdiv(rs,rt,rd,0);
}
if(strcmp(opcode, "fsqrt") == 0){
if(sscanf(op_data, fff, tmp, &rd, &rs) == 3)
return fsqrt(rs,0,rd,0);
}
if(strcmp(opcode, "fabs") == 0){
if(sscanf(op_data, fff, tmp, &rd, &rs) == 3)
return _fabs(rs,0,rd,0);
}
if(strcmp(opcode, "fmov") == 0){
if(sscanf(op_data, fff, tmp, &rd, &rs) == 3)
return fmov(rs,0,rd,0);
}
if(strcmp(opcode, "fneg") == 0){
if(sscanf(op_data, fff, tmp, &rd, &rs) == 3)
return fneg(rs,0,rd,0);
}
if(strcmp(opcode, "fldi") == 0){
if(sscanf(op_data, ffgi, tmp, &rt, &rs, &imm) == 4)
return fldi(rs,rt,imm);
}
if(strcmp(opcode, "fsti") == 0){
if(sscanf(op_data, ffgi, tmp, &rt, &rs, &imm) == 4)
return fsti(rs,rt,imm);
}
if(strcmp(opcode, "fjeq") == 0){
if(sscanf(op_data, fffl, tmp, &rs, &rt, lname) == 4) {
strcpy(label_name[label_cnt],lname);
return fjeq(rs,rt,label_cnt++);
}
}
if(strcmp(opcode, "fjlt") == 0){
if(sscanf(op_data, fffl, tmp, &rs, &rt, lname) == 4) {
strcpy(label_name[label_cnt],lname);
return fjlt(rs,rt,label_cnt++);
}
}
if(strcmp(opcode, "halt") == 0){
return halt(0,0,0,0);
}
if(strcmp(opcode, "setL") == 0){
if(sscanf(op_data, fgl, tmp, &rd, lname) == 3) {
strcpy(label_name[label_cnt],lname);
return setl(0,rd,label_cnt++);
}
}
if(strcmp(opcode, "padd") == 0){
if(sscanf(op_data, fgi, tmp, &rt, &imm) == 3) {
return padd(0,rt,imm);
}
}
if(strcmp(opcode, "link") == 0){
if(sscanf(op_data, fi, tmp, &imm) == 2) {
return link(0,0,imm);
}
}
if(strcmp(opcode, "movlr") == 0){
return movlr(0,0,0,0);
}
if(strcmp(opcode, "btmplr") == 0){
return btmplr(0,0,0,0);
}
/*
if(strcmp(opcode, "padd") == 0){
if(sscanf(op_data, fgg, tmp, &rd, &rt) == 3) {
return padd(0,rt,d,0);
}
}
*/
return -1;
}