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; }