bool RayIntersectsPlane(const ray& r, const vec3& p, const vec3& n, vec3* hit = NULL)
{
vec3 w0 = r.m_pos - p;
float a = -n.Dot(w0);
float b = n.Dot(r.m_dir);
float ep = 1e-4f;
if (fabs(b) < ep)
{
if (a == 0) // Ray is parallel
return false; // Ray is inside plane
else
return false; // Ray is somewhere else
}
float r = a/b;
if (r < 0)
return false; // Ray goes away from the plane
vec3 vecPoint = r.m_pos + r.m_dir*r;
if (hit)
*hit = vecPoint;
return true;
}
float DistanceToPlaneSqr(const vec3& vecPoint, const vec3& vecPlane, const vec3& vecPlaneNormal)
{
float sb, sn, sd;
sn = -vecPlaneNormal.Dot(vecPoint - vecPlane);
sd = vecPlaneNormal.Dot(vecPlaneNormal);
sb = sn/sd;
vec3 b = vecPoint + vecPlaneNormal * sb;
return (vecPoint - b).LengthSqr();
}
const Quaternion Quaternion::Rotation(const vec3& unitVec0, const vec3& unitVec1)
{
float cosHalfAngleX2, recipCosHalfAngleX2;
cosHalfAngleX2 = sqrt((2.0f * (1.0f + unitVec0.Dot(unitVec1))));
recipCosHalfAngleX2 = (1.0f / cosHalfAngleX2);
return Quaternion((unitVec0.Cross(unitVec1) * recipCosHalfAngleX2), (cosHalfAngleX2 * 0.5f));
}
static Matrix4<T> LookAtRH(vec3 eye, vec3 at, vec3 up)
{
vec3 viewDir = at-eye;
viewDir.Normalize();
vec3 side = viewDir.Cross(up);
side.Normalize();
vec3 newUp = side.Cross(viewDir);
newUp.Normalize();
Matrix4 m;
m.x.x = side.x; m.y.x = side.y; m.z.x = side.z; m.w.x = -eye.Dot(side);
m.x.y = newUp.x; m.y.y = newUp.y; m.z.y = newUp.z; m.w.y = -eye.Dot(newUp);
m.x.z = -viewDir.x; m.y.z = -viewDir.y; m.z.z = -viewDir.z; m.w.z = eye.Dot(viewDir);
m.x.w = 0.0f; m.y.w = 0.0f; m.z.w = 0.0f; m.w.w = 1.0f;
return m;
}
static CQuaternion CreateFromVectors(vec3 v0, vec3 v1) {
CQuaternion q;
if (v0 == -v1) {//Create from Axis Angle
//int a = 1;
}
vec3 c = v0.Cross(v1);
float d = v0.Dot(v1);
float s = (float) sqrt((1+d)*2);
q.x = c.x /s;
q.y = c.y/s;
q.z = c.z/s;
q.w = s / 2.0f;
return q;
}
bool LineSegmentIntersectsSphere(const vec3& v1, const vec3& v2, const vec3& s, float flRadius, vec3& vecPoint, vec3& vecNormal)
{
vec3 vecLine = v2 - v1;
vec3 vecSphere = v1 - s;
if (vecLine.LengthSqr() == 0)
{
if (vecSphere.LengthSqr() < flRadius*flRadius)
{
vecPoint = v1;
vecNormal = vecSphere.Normalized();
return true;
}
else
return false;
}
float flA = vecLine.LengthSqr();
float flB = 2 * vecSphere.Dot(vecLine);
float flC1 = s.LengthSqr() + v1.LengthSqr();
float flC2 = (s.Dot(v1)*2);
float flC = flC1 - flC2 - flRadius*flRadius;
float flBB4AC = flB*flB - 4*flA*flC;
if (flBB4AC < 0)
return false;
float flSqrt = sqrt(flBB4AC);
float flPlus = (-flB + flSqrt)/(2*flA);
float flMinus = (-flB - flSqrt)/(2*flA);
bool bPlusBelow0 = flPlus < 0;
bool bMinusBelow0 = flMinus < 0;
bool bPlusAbove1 = flPlus > 1;
bool bMinusAbove1 = flMinus > 1;
// If both are above 1 or below 0, then we're not touching the sphere.
if (bMinusBelow0 && bPlusBelow0 || bPlusAbove1 && bMinusAbove1)
return false;
if (bMinusBelow0 && bPlusAbove1)
{
// We are inside the sphere.
vecPoint = v1;
vecNormal = (v1 - s).Normalized();
return true;
}
if (bMinusAbove1 && bPlusBelow0)
{
// We are inside the sphere. Is this even possible? I dunno. I'm putting an assert here to see.
// If it's still here later that means no.
TAssert(false);
vecPoint = v1;
vecNormal = (v1 - s).Normalized();
return true;
}
// If flPlus is below 1 and flMinus is below 0 that means we started our trace inside the sphere and we're heading out.
// Don't intersect with the sphere in this case so that things on the inside can get out without getting stuck.
if (bMinusBelow0 && !bPlusAbove1)
return false;
// So at this point, flMinus is between 0 and 1, and flPlus is above 1.
// In any other case, we intersect with the sphere and we use the flMinus value as the intersection point.
float flDistance = vecLine.Length();
vec3 vecDirection = vecLine / flDistance;
vecPoint = v1 + vecDirection * (flDistance * flMinus);
// Oftentimes we are slightly stuck inside the sphere. Pull us out a little bit.
vec3 vecDifference = vecPoint - s;
float flDifferenceLength = vecDifference.Length();
vecNormal = vecDifference / flDifferenceLength;
if (flDifferenceLength < flRadius)
vecPoint += vecNormal * ((flRadius - flDifferenceLength) + 0.00001f);
TAssert((vecPoint - s).LengthSqr() >= flRadius*flRadius);
return true;
}
