/// <summary>光線r = p + tdが球sと交差しているかどうかを判定</summary>
/// <param name="rvP">基点</param>
/// <param name="vD">方向ベクトル</param>
/// <param name="rS">対象とする球体</param>
/// <return>交差している場合、true返す</return>
static public bool RaySphere(MCVector3 rvP, MCVector3 vD, Sphere rS)
{
MCVector3 vM = rvP - rS.c;
float fC = vM.Dot(vM) - rS.r * rS.r;
// 少なくとも1つの実数解が存在している場合、交差している
if (fC <= 0.0f)
{
return(true);
}
float fB = vM.Dot(vD);
// 光線の原点が球の外側にあり光線が球から離れた方向を指している場合には早期に終了
if (fB > 0.0f)
{
return(false);
}
float fDiscr = fB * fB - fC;
// 負の判別式は光線が球を外れていることに一致
if (fDiscr < 0.0f)
{
return(false);
}
// これで光線は球と交差している
return(true);
}
/// <summary>光線r = p + td, |vD| = 1が球sに対して交差しているかどうか。</summary>
/// <param name="rvP">基点</param>
/// <param name="vD">方向ベクトル</param>
/// <param name="rS">対象とする球体</param>
/// <param name="t">0≦ｔ≦Tmax</param>
/// <param name="hit">交差した位置</param>
/// <return>交差している場合、交差している*tの値および交差点*hitを返す</return>
public static bool IntersectRaySphere(MCVector3 rvP, MCVector3 vD, Sphere rS, out float t, out MCVector3 hit)
{
hit = new MCVector3();
t = 0;
MCVector3 vM = rvP - rS.c;
float fB = vM.Dot(vD);
float fC = vM.Dot(vM) - rS.r * rS.r;
// rの原点が*rSの外側にあり(c > 0)、rが*rSから離れていく方向を指している場合(fB > 0)に終了
if (fC > 0.0f && fB > 0.0f)
{
return(false);
}
float fDiscr = fB * fB - fC;
// 負の判別式は光線が球を外れていることに一致
if (fDiscr < 0.0f)
{
return(false);
}
// これで光線は球と交差していることが分かり、交差する最小の値*tを計算
t = -fB - (float)Mt.Sqrt(fDiscr);
// *tが負である場合、光線は球の内側から開始しているので*tをゼロにクランプ
if (t < 0.0f)
{
t = 0.0f;
}
hit = rvP + t * vD;
return(true);
}
/// <summary>与えられた点pに対して、OBB 上(もしくは中)にあるrvPの最近接点を返す</summary>
/// <param name="rvP">点</param>
/// <return>最近接点を返す</return>
public MCVector3 ClosestPtPointOBB(MCVector3 rvP)
{
MCVector3 ret;
MCVector3 vD = rvP - c;
float[] t = new float[] { e.X, e.Y, e.Z };
// 箱の中心における結果から開始、そこから段階的に進める
ret = c;
// 各OBBの軸に対して...
for (int i = 0; i < 3; i++)
{
// ...vDをその軸に射影して
// 箱の中心からvDの軸に沿った距離を得る
float fDist = vD.Dot(u[i]);
// 箱の範囲よりも距離が大きい場合、箱までクランプ
if (fDist > t[i])
{
fDist = t[i];
}
if (fDist < -t[i])
{
fDist = -t[i];
}
// ワールド座標を得るためにその距離だけ軸に沿って進める
ret += fDist * u[i];
}
return(ret);
}
/// <summary>
/// 3つの同一直線上にない点が(時計回りの順に)与えられた場合に、平面の方程式を計算
/// </summary>
/// <return>無し</return>
public void MakeComputePlane(MCVector3 vA, MCVector3 vB, MCVector3 vC)
{
vNormal = (vB - vA).Cross((vC - vA));
vNormal.Normalize();
distance = vNormal.Dot(vA);
}
/// <summary>mcAABB2D と 球体によるあたり判定</summary>
/// <param name="rS">対象とする球体</param>
/// <return>重なっている場合は trueを返し、 重なっていない場合はfalseを返す</return>
public bool AABB_Sphere(Sphere rS)
{
MCVector3 v = ClosestPtPoint(rS.c);
v -= rS.c;
return(v.Dot() <= rS.r * rS.r);
}
/// <summary>球sがOBB bに交差している場合は真を返し、そうでなければ偽を返す</summary>
/// 球の中心に対するOBB上の最近接点である点pも返す
/// <param name="rS">球体</param>
/// <param name="hit">最近接点</param>
/// <return>true false</return>
public bool SphereOBB(Sphere rS, out MCVector3 hit)
{
// 球の中心に対する最近接点であるOBB上にある点pを見つける
hit = ClosestPtPointOBB(rS.c);
// 球とOBBが交差するのは、球の中心から点pまでの(平方した)距離が
// (平方した)球の半径よりも小さい場合
MCVector3 v = hit - rS.c;
return(v.Dot() <= rS.r * rS.r);
}
/// <summary>
/// この平面に対してもう一つの平面rPlaneに対して、それらの交差である直線
/// L = pOutP + t * pOutN
/// を計算し、直線が存在しない場合はfalseを返す
/// 平面の法線は正規化されている物とする。
/// </summary>
/// <param name="plane">平面</param>
/// <param name="vN">交差直線の方向</param>
/// <param name="vP">交差直線上の点の位置</param>
/// <return>直線が存在しない場合はfalseを返す。</return>
public bool IntersectPlanes(MCPlane3 plane, out MCVector3 vN, out MCVector3 vP)
{
// 交差直線の方向を計算
vN = vNormal.Cross(plane.vNormal);
vP = new MCVector3();
// pOutNが０の場合、平面は平行か離れている
// あるいは一致しているので、交差しているとは考えられない
float fDenom = vN.Dot();
if (fDenom < 0.0001f)
{
return(false);
}
// 交差直線上の点の位置
vP = vN.Cross((distance * plane.vNormal - plane.distance * vNormal));
vP /= fDenom;
return(true);
}
/// <summary>自身のOBB と OBB(rB)によるあたり判定</summary>
/// <param name="rB">OBB3D構造体 B</param>
/// <return>重なっている場合は 1を返し、 重なっていない場合は０を返す</return>
public bool OBB_OBB(OBB3D rB)
{
float fRA, fRB;
u = new MCVector3[3];
MCMatrix4x4 mR = MCMatrix4x4.Identity, mAbsR = MCMatrix4x4.Identity;
// aの座標フレームの中でbを表現する回転行列を計算
mR.M11 = u[0].Dot(rB.u[0]);
mR.M12 = u[0].Dot(rB.u[1]);
mR.M13 = u[0].Dot(rB.u[2]);
mR.M21 = u[1].Dot(rB.u[0]);
mR.M22 = u[1].Dot(rB.u[1]);
mR.M23 = u[1].Dot(rB.u[2]);
mR.M31 = u[2].Dot(rB.u[0]);
mR.M32 = u[2].Dot(rB.u[1]);
mR.M33 = u[2].Dot(rB.u[2]);
// 平行移動ベクトルvTを計算計算
MCVector3 vT = rB.c - c;
// 平行移動をaの座標フレームに変換
vT = new MCVector3(vT.Dot(u[0]), vT.Dot(u[1]), vT.Dot(u[2]));
// 共通の部分式を計算。
// 2つの辺が平行でそれらの外積がゼロベクトル(あるいはそれに近いベクトル)になる時に
// 演算エラーが起きないようにイプシロンの項を追加(詳しくは本文を参照)
mAbsR.M11 = Mt.Abs(mR.M11) + 0.0001f;
mAbsR.M12 = Mt.Abs(mR.M12) + 0.0001f;
mAbsR.M13 = Mt.Abs(mR.M13) + 0.0001f;
mAbsR.M21 = Mt.Abs(mR.M21) + 0.0001f;
mAbsR.M22 = Mt.Abs(mR.M22) + 0.0001f;
mAbsR.M23 = Mt.Abs(mR.M23) + 0.0001f;
mAbsR.M31 = Mt.Abs(mR.M31) + 0.0001f;
mAbsR.M32 = Mt.Abs(mR.M32) + 0.0001f;
mAbsR.M33 = Mt.Abs(mR.M33) + 0.0001f;
// 軸L = A0, L = A1, L = A2を判定
{
fRA = e.X;
fRB = rB.e.X * mAbsR.M11 + rB.e.Y * mAbsR.M12 + rB.e.Z * mAbsR.M13;
if (Mt.Abs(vT.X) > fRA + fRB)
{
return(false);
}
//
fRA = e.Y;
fRB = rB.e.X * mAbsR.M21 + rB.e.Y * mAbsR.M22 + rB.e.Z * mAbsR.M23;
if (Mt.Abs(vT.Y) > fRA + fRB)
{
return(false);
}
//
fRA = e.Z;
fRB = rB.e.X * mAbsR.M31 + rB.e.Y * mAbsR.M32 + rB.e.Z * mAbsR.M33;
if (Mt.Abs(vT.Z) > fRA + fRB)
{
return(false);
}
}
// 軸L = B0, L = B1, L = B2を判定
{
fRA = e.X * mAbsR.M11 + e.Y * mAbsR.M21 + e.Z * mAbsR.M31;
fRB = rB.e.X;
if (Mt.Abs(vT.X * mR.M11 + vT.Y * mR.M21 + vT.Z * mR.M31) > fRA + fRB)
{
return(false);
}
//
fRA = e.X * mAbsR.M12 + e.Y * mAbsR.M22 + e.Z * mAbsR.M32;
fRB = rB.e.Y;
if (Mt.Abs(vT.X * mR.M12 + vT.Y * mR.M22 + vT.Z * mR.M32) > fRA + fRB)
{
return(false);
}
//
fRA = e.X * mAbsR.M13 + e.Y * mAbsR.M23 + e.Z * mAbsR.M33;
fRB = rB.e.Z;
if (Mt.Abs(vT.X * mR.M13 + vT.Y * mR.M23 + vT.Z * mR.M33) > fRA + fRB)
{
return(false);
}
}
// 軸L = A0 X B0を判定
fRA = e.Y * mAbsR.M31 + e.Z * mAbsR.M21;
fRB = rB.e.Y * mAbsR.M13 + rB.e.Z * mAbsR.M12;
if (Mt.Abs(vT.Z * mR.M21 - vT.Y * mR.M31) > fRA + fRB)
{
return(false);
}
// 軸L = A0 X B1を判定
fRA = e.Y * mAbsR.M32 + e.Z * mAbsR.M22;
fRB = rB.e.X * mAbsR.M13 + rB.e.Z * mAbsR.M11;
if (Mt.Abs(vT.Z * mR.M22 - vT.Y * mR.M32) > fRA + fRB)
{
return(false);
}
// 軸L = A0 X B2を判定
fRA = e.Y * mAbsR.M33 + e.Z * mAbsR.M23;
fRB = rB.e.X * mAbsR.M12 + rB.e.Y * mAbsR.M11;
if (Mt.Abs(vT.Z * mR.M23 - vT.Y * mR.M33) > fRA + fRB)
{
return(false);
}
// 軸L = A1 X B0を判定
fRA = e.X * mAbsR.M31 + e.Z * mAbsR.M11;
fRB = rB.e.Y * mAbsR.M23 + rB.e.Z * mAbsR.M22;
if (Mt.Abs(vT.X * mR.M31 - vT.Z * mR.M11) > fRA + fRB)
{
return(false);
}
// 軸L = A1 X B1を判定
fRA = e.X * mAbsR.M32 + e.Z * mAbsR.M12;
fRB = rB.e.X * mAbsR.M23 + rB.e.Z * mAbsR.M21;
if (Mt.Abs(vT.X * mR.M32 - vT.Z * mR.M12) > fRA + fRB)
{
return(false);
}
// 軸L = A1 X B2を判定
fRA = e.X * mAbsR.M33 + e.Z * mAbsR.M13;
fRB = rB.e.X * mAbsR.M22 + rB.e.Y * mAbsR.M21;
if (Mt.Abs(vT.X * mR.M33 - vT.Z * mR.M13) > fRA + fRB)
{
return(false);
}
// 軸L = A2 X B0を判定
fRA = e.X * mAbsR.M21 + e.Y * mAbsR.M11;
fRB = rB.e.Y * mAbsR.M33 + rB.e.Z * mAbsR.M32;
if (Mt.Abs(vT.Y * mR.M11 - vT.X * mR.M21) > fRA + fRB)
{
return(false);
}
// 軸L = A2 X B1を判定
fRA = e.X * mAbsR.M22 + e.Y * mAbsR.M12;
fRB = rB.e.X * mAbsR.M33 + rB.e.Z * mAbsR.M31;
if (Mt.Abs(vT.Y * mR.M12 - vT.X * mR.M22) > fRA + fRB)
{
return(false);
}
// 軸L = A2 X B2を判定
fRA = e.X * mAbsR.M23 + e.Y * mAbsR.M13;
fRB = rB.e.X * mAbsR.M32 + rB.e.Y * mAbsR.M31;
if (Mt.Abs(vT.Y * mR.M13 - vT.X * mR.M23) > fRA + fRB)
{
return(false);
}
// 分離軸が見つからないので、OBBは交差している
return(true);
}
/// <summary>
/// 与えられた厚みのイプシロンにより厚みのある平面に対して点vPを分類
/// </summary>
/// <param name="vP">基点</param>
/// <param name="planeThicknessEpsilon">イプシロン値</param>
/// <return>平面に対しての点の位置を返す</return>
public POSITION_PLANE_POINT ClassifyPointToPlane(MCVector3 vP, float planeThicknessEpsilon)
{
//================================
// 点の平面からの符号付距離を計算
//================================
float fDist = vNormal.Dot(vP) - distance;
//================================
// 符号付距離を基にしてvPを分類
//================================
if (fDist > planeThicknessEpsilon)
{
// 平面の厚さのイプシロン値より大きかった
return(POSITION_PLANE_POINT.POINT_IN_FRONT_OF_PLANE);
}
else if (fDist < -planeThicknessEpsilon)
{
// 平面の厚さのイプシロン値より小さかった
return(POSITION_PLANE_POINT.POINT_BEHIND_PLANE);
}
return(POSITION_PLANE_POINT.POINT_ON_PLANE);
}