void R3DCamera::Rotate(float angle, float x, float y, float z)
{
// Get direction vector
T_GLVector3f vecDir, vecNew;
Vec3fSub(&vecDir, this->vLookAt, this->vEye);
// Compute sin and cos of the angle
float sinAng = (float)sin(angle);
float cosAng = (float)cos(angle);
// Compute new x position
vecNew.x = (cosAng + (1 - cosAng) * x * x) * vecDir.x;
vecNew.x += ((1 - cosAng) * x * y - z * sinAng) * vecDir.y;
vecNew.x += ((1 - cosAng) * x * z + y * sinAng) * vecDir.z;
// Compute new y position
vecNew.y = ((1 - cosAng) * x * y + z * sinAng) * vecDir.x;
vecNew.y += (cosAng + (1 - cosAng) * y * y) * vecDir.y;
vecNew.z += ((1 - cosAng) * y * z - x * sinAng) * vecDir.z;
// Compute new z position
vecNew.z = ((1 - cosAng) * x * z - y * sinAng) * vecDir.x;
vecNew.z += ((1 - cosAng) * y * z + x * sinAng) * vecDir.y;
vecNew.z += (cosAng + (1 - cosAng) * z * z) * vecDir.z;
// Compute new rotated look at vector
Vec3fAdd(&this->vLookAt, this->vEye, vecNew);
}
bool IntersectSpherefSpheref(const Point3f center1, F32 radius1, const Point3f center2, F32 radius2) {
Vec3f d;
Vec3fSub(center1.point, center2.point, &d);
F32 dist2;
dist2 = Vec3fDot(d,d);
//intersect if squared distance is less than squared sum of radius
F32 radiusSum = radius1 + radius2;
return dist2 <= radiusSum * radiusSum;
}
bool IntersectRaySpheref( const Point3f& origin, const Direction3f& direction, const Point3f& centerSph, F32 radius, F32& t, Point3f& q ) {
Vec3f m;
Vec3fSub( origin.point , centerSph.point, &m );
F32 b = Vec3fDot( m , direction.dir );
F32 c = Vec3fDot( m , m ) - radius * radius;
// Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
if(c > 0.f && b > 0.f)
return false;
F32 discr = b * b - c;
// A negative discriminant corresponds to ray missing sphere
if (discr < 0.f)
return false;
// Ray now found to intersect sphere, compute smallest t value of intersection
t = -b - Sqrtf(discr);
// If t is negative, ray started inside sphere so clamp t to zero
if (t < 0.f)
t = 0.f;
Vec3f tempRes = direction.dir;
Vec3fMultScalarInplace( t , &tempRes );
Vec3fSumInplace( origin.point , &tempRes );
q.point = tempRes;
return true;
}
//to do: try this...
bool IntersectRayOBB3f(const Ray3f& ray, const OBB3f& obb) {
//intersect ray-obb reference: http://www.geometrictools.com/LibMathematics/Intersection/Intersection.html intersect box ray
//bool IntrRay3Box3<Real>::Test ()
//{
//Real WdU[3], AWdU[3], DdU[3], ADdU[3], AWxDdU[3], RHS;
F32 WdU[3], AWdU[3], DdU[3], ADdU[3], AWxDdU[3], RHS;
//Vector3<Real> diff = mRay->Origin - mBox->Center;
Vec3f diff;
Vec3fSub(ray.origin.point, obb.center.point, &diff);
//WdU[0] = mRay->Direction.Dot(mBox->Axis[0]);
WdU[0] = Vec3fDot(ray.direction.dir, obb.orientX);
//AWdU[0] = Math<Real>::FAbs(WdU[0]);
AWdU[0] = Absf(WdU[0]);
//DdU[0] = diff.Dot(mBox->Axis[0]);
DdU[0] = Vec3fDot(diff, obb.orientX);
//ADdU[0] = Math<Real>::FAbs(DdU[0]);
ADdU[0] = Absf(DdU[0]);
//if (ADdU[0] > mBox->Extent[0] && DdU[0]*WdU[0] >= (Real)0)
if ( ADdU[0] > obb.halfWidths.x && DdU[0]*WdU[0] >= 0.f) {
return false;
}
//WdU[1] = mRay->Direction.Dot(mBox->Axis[1]);
WdU[1] = Vec3fDot(ray.direction.dir, obb.orientY);
//AWdU[1] = Math<Real>::FAbs(WdU[1]);
AWdU[1] = Absf(WdU[1]);
//DdU[1] = diff.Dot(mBox->Axis[1]);
DdU[1] = Vec3fDot(diff, obb.orientY);
//ADdU[1] = Math<Real>::FAbs(DdU[1]);
ADdU[1] = Absf(DdU[1]);
//if (ADdU[1] > mBox->Extent[1] && DdU[1]*WdU[1] >= (Real)0)
if ( ADdU[1] > obb.halfWidths.y && DdU[1]*WdU[1] >= 0.f) {
return false;
}
//WdU[2] = mRay->Direction.Dot(mBox->Axis[2]);
WdU[2] = Vec3fDot(ray.direction.dir, obb.orientZ);
//AWdU[2] = Math<Real>::FAbs(WdU[2]);
AWdU[2] = Absf(WdU[2]);
//DdU[2] = diff.Dot(mBox->Axis[2]);
DdU[2] = Vec3fDot(diff, obb.orientZ);
//ADdU[2] = Math<Real>::FAbs(DdU[2]);
ADdU[2] = Absf(DdU[2]);
//if (ADdU[2] > mBox->Extent[2] && DdU[2]*WdU[2] >= (Real)0)
if ( ADdU[2] > obb.halfWidths.z && DdU[2]*WdU[2] >= 0.f) {
return false;
}
//Vector3<Real> WxD = mRay->Direction.Cross(diff);
Vec3f WxD;
Vec3fCross(ray.direction.dir, diff, &WxD);
//AWxDdU[0] = Math<Real>::FAbs(WxD.Dot(mBox->Axis[0]));
AWxDdU[0] = Absf(Vec3fDot(WxD, obb.orientX));
//RHS = mBox->Extent[1]*AWdU[2] + mBox->Extent[2]*AWdU[1];
RHS = obb.halfWidths.y * AWdU[2] + obb.halfWidths.z * AWdU[1];
if (AWxDdU[0] > RHS)
{
return false;
}
//AWxDdU[1] = Math<Real>::FAbs(WxD.Dot(mBox->Axis[1]));
AWxDdU[1] = Absf(Vec3fDot(WxD, obb.orientY));
//RHS = mBox->Extent[0]*AWdU[2] + mBox->Extent[2]*AWdU[0];
RHS = obb.halfWidths.x * AWdU[2] + obb.halfWidths.z * AWdU[0];
if (AWxDdU[1] > RHS)
{
return false;
}
//AWxDdU[2] = Math<Real>::FAbs(WxD.Dot(mBox->Axis[2]));
AWxDdU[2] = Absf(Vec3fDot(WxD, obb.orientZ));
//RHS = mBox->Extent[0]*AWdU[1] + mBox->Extent[1]*AWdU[0];
RHS = obb.halfWidths.x * AWdU[1] + obb.halfWidths.y * AWdU[0];
if (AWxDdU[2] > RHS)
{
return false;
}
return true;
}
bool IntersectRayTriangle3f(const Ray3f& ray, const Triangle3f& tr) {
//reference: Essential Mathematics for Games and Interactive pag. 585
// test ray direction against triangle
//triangle' s point: v1=a,v0=b,v2=c
Vec3f v0 = Vec3f(tr.b.point.x, tr.b.point.y, tr.b.point.z);
Vec3f v1 = Vec3f(tr.a.point.x, tr.a.point.y, tr.a.point.z);
Vec3f v2 = Vec3f(tr.c.point.x, tr.c.point.y, tr.c.point.z);
//e1 = v1 - v0;
Vec3f e1;
Vec3fSub(v1, v0, &e1);
//e2 = v2 - v0;
Vec3f e2;
Vec3fSub(v2, v0, &e2);
//p = ray.mDirection.Cross(e2);
Vec3f p;
Vec3fCross(ray.direction.dir, e2, &p);
//float a = e1.Dot(p);
F32 a = Vec3fDot(e1, p);
// if result zero, no intersection or infinite intersections
// (ray parallel to triangle plane)
//if ( IsZerof(a) )
if (IsZerof(a))
return false;
// compute denominator
F32 f = 1.0f/a;
// compute barycentric coordinates
//IvVector3 s = ray.mOrigin - v0;
Vec3f s;
Vec3f mOrigin = Vec3f(ray.origin.point.x, ray.origin.point.y, ray.origin.point.z);
Vec3fSub(mOrigin, v0, &s);
//u = f*s.Dot(p)
F32 u = f * Vec3fDot(s, p);
if (u < 0.0f || u > 1.0f)
return false;
//q = s.Cross(e1);
Vec3f q;
Vec3fCross(s, e1, &q);
//v = f*ray.mDirection.Dot(q);
F32 v = f * Vec3fDot(ray.direction.dir, q);
if (v < 0.0f || u+v > 1.0f)
return false;
// compute line parameter
//t = f*e2.Dot(q);
F32 t = f * Vec3fDot(e2, q);
return (t >= 0.0f);
}