public static float SqrDistance(ref Vector3 rkPoint, ref Ray3 rkRay) { Vector3 kDiff = rkPoint - rkRay.Origin; float fT = kDiff.dot(ref rkRay.Direction); if (fT <= 0.0) { fT = 0.0f; } else { fT /= rkRay.Direction.length2(); kDiff -= fT * rkRay.Direction; } return(kDiff.length2()); }
public static float SqrDistance(ref Ray3 rkRay, ref Segment3 rkSeg, out float pfRayP, out float pfSegP) { Vector3 kDiff = rkRay.Origin - rkSeg.Origin; float fA00 = rkRay.Direction.length2(); float fA01 = -rkRay.Direction.dot(ref rkSeg.Direction); float fA11 = rkSeg.Direction.length2(); float fB0 = kDiff.dot(ref rkRay.Direction); float fC = kDiff.length2(); float fDet = System.Math.Abs(fA00 * fA11 - fA01 * fA01); float fB1, fS, fT, fSqrDist, fTmp; if (fDet >= gs_fTolerance) { // ray and segment are not parallel fB1 = -kDiff.dot(ref rkSeg.Direction); fS = fA01 * fB1 - fA11 * fB0; fT = fA01 * fB0 - fA00 * fB1; if (fS >= 0.0) { if (fT >= 0.0) { if (fT <= fDet) // region 0 { // minimum at interior points of ray and segment float fInvDet = 1.0f / fDet; fS *= fInvDet; fT *= fInvDet; fSqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) + fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC; } else // region 1 { fT = 1.0f; if (fB0 >= -fA01) { fS = 0.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fTmp = fA01 + fB0; fS = -fTmp / fA00; fSqrDist = fTmp * fS + fA11 + 2.0f * fB1 + fC; } } } else // region 5 { fT = 0.0f; if (fB0 >= 0.0) { fS = 0.0f; fSqrDist = fC; } else { fS = -fB0 / fA00; fSqrDist = fB0 * fS + fC; } } } else { if (fT <= 0.0) // region 4 { if (fB0 < 0.0) { fS = -fB0 / fA00; fT = 0.0f; fSqrDist = fB0 * fS + fC; } else { fS = 0.0f; if (fB1 >= 0.0) { fT = 0.0f; fSqrDist = fC; } else if (-fB1 >= fA11) { fT = 1.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fT = -fB1 / fA11; fSqrDist = fB1 * fT + fC; } } } else if (fT <= fDet) // region 3 { fS = 0.0f; if (fB1 >= 0.0) { fT = 0.0f; fSqrDist = fC; } else if (-fB1 >= fA11) { fT = 1.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fT = -fB1 / fA11; fSqrDist = fB1 * fT + fC; } } else // region 2 { fTmp = fA01 + fB0; if (fTmp < 0.0) { fS = -fTmp / fA00; fT = 1.0f; fSqrDist = fTmp * fS + fA11 + 2.0f * fB1 + fC; } else { fS = 0.0f; if (fB1 >= 0.0) { fT = 0.0f; fSqrDist = fC; } else if (-fB1 >= fA11) { fT = 1.0f; fSqrDist = fA11 + 2 * fB1 + fC; } else { fT = -fB1 / fA11; fSqrDist = fB1 * fT + fC; } } } } } else { // ray and segment are parallel if (fA01 > 0.0) { // opposite direction vectors fT = 0.0f; if (fB0 >= 0.0) { fS = 0.0f; fSqrDist = fC; } else { fS = -fB0 / fA00; fSqrDist = fB0 * fS + fC; } } else { // same direction vectors fB1 = -kDiff.dot(ref rkSeg.Direction); fT = 1.0f; fTmp = fA01 + fB0; if (fTmp >= 0.0) { fS = 0.0f; fSqrDist = fA11 + 2.0f * fB1 + fC; } else { fS = -fTmp / fA00; fSqrDist = fTmp * fS + fA11 + 2.0f * fB1 + fC; } } } pfRayP = fS; pfSegP = fT; return(System.Math.Abs(fSqrDist)); }
/// <summary> /// Compute the angle between two vectors. /// </summary> /// <param name="v">The other vector.</param> /// <returns></returns> public float angle(ref Vector3 v) { float s = (float)System.Math.Sqrt(length2() * v.length2()); return((float)System.Math.Acos(dot(ref v) / s)); }