/// <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>点rPと、このmcAABB2Dの間の距離の平方を計算</summary>
/// <param name="rP">与えられた点</param>
/// <return>距離の平方値</return>
public float SqDistPoint(MCVector3 rP)
{
float sqDist = 0.0f;
if (rP.X < vMin.X)
{
sqDist += (vMin.X - rP.X) * (vMin.X - rP.X);
}
if (rP.Y < vMin.Y)
{
sqDist += (vMin.Y - rP.Y) * (vMin.X - rP.Y);
}
if (rP.Z < vMin.Z)
{
sqDist += (vMin.Z - rP.Z) * (vMin.Z - rP.Z);
}
if (rP.X > vMax.X)
{
sqDist += (vMax.X - rP.X) * (vMax.X - rP.X);
}
if (rP.Y > vMax.Y)
{
sqDist += (vMax.Y - rP.Y) * (vMax.X - rP.Y);
}
if (rP.Z > vMax.Z)
{
sqDist += (vMax.Z - rP.Z) * (vMax.Z - rP.Z);
}
return(sqDist);
}
/// <summary>3つの頂点からmcAABB2Dを作成</summary>
/// <param name="vA">頂点A</param>
/// <param name="vB">頂点B</param>
/// <param name="vC">頂点C</param>
public void MakeFromeTriangle(MCVector3 vA, MCVector3 vB, MCVector3 vC)
{
vMin.InitMin();
vMax.InitMax();
vMin.SetMin(vA); vMin.SetMin(vB); vMin.SetMin(vC);
vMax.SetMax(vA); vMax.SetMax(vB); vMax.SetMax(vC);
}
/// <summary>2つの頂点からmcAABB2Dを作成</summary>
/// <param name="vA">頂点A</param>
/// <param name="vB">頂点B</param>
public void MakeFromeSegment(MCVector3 vA, MCVector3 vB)
{
vMin.InitMin();
vMax.InitMax();
vMin.SetMin(vA); vMin.SetMin(vB);
vMax.SetMax(vA); vMax.SetMax(vB);
}
/// <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>
/// ベクトルrV を 単位四元数(this)によって回転します。
/// 戻り値 = (this) * v * ~(this);
/// </summary>
/// <param name="v">３要素ベクトル</param>
/// <return>位四元数(this)によって回転したベクトル</return>
public MCVector3 VecRotate(MCVector3 v)
{
MCVector3 o;
VecRotate(out o, v);
return(o);
}
/// <summary>与えられた点rPに対して、このmcAABB2Dのうえもしくは中にあるrPの最近接点を返す</summary>
/// <param name="rP">与えられた点</param>
/// <return>最近接点</return>
public MCVector3 ClosestPtPoint(MCVector3 rP)
{
MCVector3 ret = new MCVector3();
if (rP.X < vMin.X)
{
ret.X = vMin.X;
}
if (rP.Y < vMin.Y)
{
ret.Y = vMin.Y;
}
if (rP.Z < vMin.Z)
{
ret.Z = vMin.Z;
}
if (rP.X > vMax.X)
{
ret.X = vMax.X;
}
if (rP.Y > vMax.Y)
{
ret.Y = vMax.Y;
}
if (rP.Z > vMax.Z)
{
ret.Z = vMax.Z;
}
return(ret);
}
/// <summary>一定の速度rvMeおよびrvBでそれぞれ運動しているこの自身のmcAABB2Dおよび'aabb'が交差するか? @n</summary>
/// 交差する場合には、最初および最後の接触時間がpfTFirstおよびpfTLastに返る
/// <param name="aabb">対象とするmcAABB2D</param>
/// <param name="rvMe">自身のmcAABB2Dの速度</param>
/// <param name="rvB">aabbの速度</param>
/// <param name="rTFirst">最初の接触時間が返される</param>
/// <param name="pfTLast">最後の接触時間が返される</param>
/// <return>重なっている場合は trueを返し、 重なっていない場合はfalseを返す</return>
public bool IntersectMovingAABBAABB(
AABB3D aabb,
MCVector3 rvMe, MCVector3 rvB,
float rTFirst, float rTLast)
{
return(IntersectMovingAABBAABB(aabb, rvMe, rvB, rTFirst, rTLast));
}
/// <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>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>光線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>
/// vDの方向に時間間隔fT0 <= *t <= fT1の間だけ運動している球 s0
/// s0（この球体）
/// </summary>
/// <param name="vD">s0の方向ベクトル</param>
/// <param name="fT0">最小時間</param>
/// <param name="fT1"最大時間</param>
/// <param name="s1">球体１</param>
/// <param name="t">時間を返す</param>
/// <returns></returns>
bool MovingSphereSphereTime(MCVector3 vD, float fT0, float fT1, Sphere s1, out float t)
{
// 時間間隔fT0からfT1までの間に、*pS0の運動している球の境界を計算
Sphere b;
t = 0;
float fMid = (fT0 + fT1) * 0.5f;
b.c = this.c + vD * fMid;
b.r = (fMid - fT0) * vD.Length() + this.r;
// 境界球がs1と重ならない場合、従ってこの時間間隔では衝突はない。
if (!b.SphereSphere(s1))
{
return(false);
}
// 衝突を除外することはできない。より精密な判定のために再帰的に判定が行われる、
// 再帰を停止するために、時間間隔が十分に小さくなった時に衝突が仮定される
if (fT1 - fT0 < 0.0001f)
{
t = fT0;
return(true);
}
// 間隔の前半部分の半分における判定を再帰的に行い、衝突が検知された場合は戻る
if (MovingSphereSphereTime(vD, fT0, fMid, s1, out t))
{
return(true);
}
// 間隔の後半部分の半分における判定を再帰的に行う
return(MovingSphereSphereTime(vD, fMid, fT1, s1, out t));
}
/// <summary>対象位置に、各軸の長さだけのmcAABB2Dから移動した時のmcAABB2Dを作る</summary>
/// <param name="len">mcAABB2Dの各軸の半分の長さ</param>
/// <param name="basePos">元となる現在位置</param>
/// <param name="v">rBasePosを原点とした運動ベクトル</param>
public void MakeMovePoint(MCVector3 len, MCVector3 basePos, MCVector3 v)
{
InitMinMax();
vMin.SetMin(basePos - len);
vMax.SetMax(basePos + len);
vMin.SetMin((basePos + v) - len);
vMax.SetMax((basePos + v) + len);
}
/// <summary>
/// マトリックスと3要素のベクトルを掛ける
/// </summary>
/// <param name="v">3要素のベクトル</param>
/// <return>演算結果を返す</return>
public MCVector3 Multiply(MCVector3 v)
{
MCVector3 o = new MCVector3();
o.X = (M11 * v.X) + (M12 * v.Y);
o.Y = (M21 * v.X) + (M22 * v.Y);
return(o);
}
/// <summary>
/// マトリックスと3要素のベクトルを掛ける
/// </summary>
/// <param name="v">3要素のベクトル</param>
/// <return>演算結果を返す</return>
public MCVector3 Multiply(MCVector3 v)
{
MCVector3 o = new MCVector3();
o.X = (M11 * v.X) + (M12 * v.Y) + (M13 * v.Z);
o.Y = (M21 * v.X) + (M22 * v.Y) + (M23 * v.Z);
o.Z = (M31 * v.X) + (M32 * v.Y) + (M33 * v.Z);
return(o);
}
/// <summary>
/// ベクトルrV を 単位四元数(this)によって回転します。
/// 戻り値 = (this) * rV * ~(this);
/// </summary>
/// <param name="o">単位四元数(this)によって回転したベクトル</param>
/// <param name="v">３要素ベクトル</param>
/// <return>pOutと同じポインタ</return>
public void VecRotate(out MCVector3 o, MCVector3 v)
{
MCQuaternion qTmp;
qTmp = this * v * ~(this);
o.X = qTmp.X;
o.Y = qTmp.Y;
o.Z = qTmp.Z;
}
/// <summary>
/// 正規化する
/// </summary>
/// <return>無し</return>
public void Normalize()
{
float f = 1.0f / vNormal.Length();
if (f != 0.0f)
{
vNormal *= f; distance *= f;
}
}
/// <summary>
/// 任意の軸を回転軸としてクォータニオンを回転させます。
/// </summary>
/// <param name="v">クォータニオンの回転軸を指定する、MCVector3 構造体へのポインタ。</param>
/// <param name="angle">回転の角度 (ラジアン単位)。</param>
/// <return>なし</return>
public void MakeRotationAxis(MCVector3 v, float angle)
{
float fHalfAngle = 0.5f * angle;
float fSin = (float)System.Math.Sin(fHalfAngle);
W = (float)System.Math.Cos(fHalfAngle);
X = fSin * v.X;
Y = fSin * v.Y;
Z = fSin * v.Z;
}
/// <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>mcAABB2Dから8頂点に分解してpvOutにセットする</summary>
/// <param name="pvOut">8個分頂点データがあるポインタ</param>
/// <return>なし</return>
public void Get8Vertexs(out MCVector3[] pvOut)
{
pvOut = new MCVector3[8];
pvOut[0].X = vMin.X; pvOut[1].X = vMax.X; pvOut[2].X = vMax.X; pvOut[3].X = vMin.X;
pvOut[0].Y = vMin.Y; pvOut[1].Y = vMin.Y; pvOut[2].Y = vMin.Y; pvOut[3].Y = vMin.Y;
pvOut[0].Z = vMin.Z; pvOut[1].Z = vMin.Z; pvOut[2].Z = vMax.Z; pvOut[3].Z = vMax.Z;
//----
pvOut[4].X = vMin.X; pvOut[5].X = vMax.X; pvOut[6].X = vMax.X; pvOut[7].X = vMin.X;
pvOut[4].Y = vMax.Y; pvOut[5].Y = vMax.Y; pvOut[6].Y = vMax.Y; pvOut[7].Y = vMax.Y;
pvOut[4].Z = vMin.Z; pvOut[5].Z = vMin.Z; pvOut[6].Z = vMax.Z; pvOut[7].Z = vMax.Z;
}
/// <summary>対象mcAABB2Dに基本位置を足したmcAABB2Dが移動した時のmcAABB2Dを作る</summary>
/// <param name="aabbBase">元となるmcAABB2D</param>
/// <param name="basePos">元となる現在位置</param>
/// <param name="vel">rBasePosを原点とした運動ベクトル</param>
/// <return>なし</return>
public void MakeMoveAABB(AABB3D aabbBase, MCVector3 basePos, MCVector3 vel)
{
AABB3D tmp;
vMin = aabbBase.vMin + basePos;
vMax = aabbBase.vMax + basePos;
tmp = this;
tmp.vMin += vel;
tmp.vMax += vel;
vMin.SetMin(tmp.vMin);
vMax.SetMax(tmp.vMax);
}
/// <summary>対象mcAABB2Dに基本位置を足したmcAABB2Dが移動した時のmcAABB2Dを作る</summary>
/// <param name="baseSphere">元となる球体</param>
/// <param name="basePos">rBasePosを原点とした運動ベクトル</param>
public void MakeMoveAABB(Sphere baseSphere, MCVector3 basePos)
{
AABB3D tmp;
vMin = baseSphere.c - baseSphere.r;
vMax = baseSphere.c + baseSphere.r;
tmp = this;
tmp.vMin += basePos;
tmp.vMax += basePos;
vMin.SetMin(tmp.vMin);
vMax.SetMax(tmp.vMax);
}
/// <summary>AABB から OBBを作る。</summary>
/// <param name="rMin">最小点</param>
/// <param name="rMax">最大点</param>
/// <return>無し</return>
public void MakeOBB_AABB(MCVector3 rMin, MCVector3 rMax)
{
// 中心点
c = (rMin + rMax) * 0.5f;
// OBBの各軸に沿って正の幅の半分の範囲
e = rMax - c;
e.X = Mt.Abs(e.X);
e.Y = Mt.Abs(e.Y);
e.Z = Mt.Abs(e.Z);
// ローカルX, Y, およびZ軸
u[0] = new MCVector3(1.0f, 0.0f, 0.0f);
u[1] = new MCVector3(0.0f, 1.0f, 0.0f);
u[2] = new MCVector3(0.0f, 0.0f, 1.0f);
}
/// <summary>指定した点(rP)が、自身のmcAABB2D内に存在するか？</summary>
/// <param name="rP">対象とする点</param>
/// <return>重なっている場合は trueを返し、 重なっていない場合はfalseを返す</return>
public bool AABB_Point(MCVector3 rP)
{
// ある軸に沿って分離している場合は交差がないものとして終了
if (vMax.X < rP.X || vMin.X > rP.X)
{
return(false);
}
if (vMax.Y < rP.Y || vMin.Y > rP.Y)
{
return(false);
}
if (vMax.Y < rP.Z || vMin.Y > rP.Z)
{
return(false);
}
// すべての軸に沿って重なっている場合にmcAABB2Dは交差している
return(true);
}
/// <summary>
/// 線分ABが平面p(this)と交差しているかどうかを判定。交差していれば交差点を返す
/// </summary>
/// <param name="vA">始点</param>
/// <param name="vB">終点</param>
/// <param name="time">平面と交差する方向のある直線abと交差する値</param>
/// <param name="vIntersect">交差点</param>
/// <return>交差の値tおよび交差点Qとともにtrueを返す。そうでなければfalseを返す</return>
public bool IntersectSegment(MCVector3 vA, MCVector3 vB, out float time, out MCVector3 vIntersect)
{
// 平面と交差する方向のある直線abと交差するtの値を計算
MCVector3 vAB = vB - vA;
vIntersect = new MCVector3();
time = (distance - vNormal.Dot(vA)) / vNormal.Dot(vAB);
// tが[0..1]の中にある場合、交差点を計算して返す
if (time >= 0.0f && time <= 1.0f)
{
vIntersect = vA + (vAB * time);
return(true);
}
// そうでない場合tは+INF, -INF, NaN, あるいは[0..1]の中にはないので、交差なし
return(false);
}
/// <summary>
/// クォータニオンのベクトル部分が表す回転の軸に沿った単位ベクトルを返す。
/// </summary>
/// <return>単位ベクトルを返す。</return>
public MCVector3 GetAxis()
{
MCVector3 vRet;
float fLSq;
vRet = new MCVector3(X, Y, Z);
fLSq = vRet.Length();
if (fLSq <= Q_EPSILON)
{
vRet.Init();
return(vRet);
}
else
{
fLSq = 1.0f / fLSq;
return(vRet * fLSq);
}
}
/// <summary>
/// 線分ABが平面p(this)と交差しているかどうかを判定。交差していれば交差点を返す
/// </summary>
/// <param name="vA">始点</param>
/// <param name="vVel">ベロシティー</param>
/// <param name="fEpsilon">平面の厚み</param>
/// <param name="time">平面と交差する方向のある直線abと交差する値</param>
/// <param name="vIntersect">交差点</param>
/// <return>交差の値tおよび交差点Qとともにtrueを返す。そうでなければfalseを返す</return>
public int IntersectLine(MCVector3 vA, MCVector3 vVel, float fEpsilon, out float time, out MCVector3 vIntersect)
{
time = 0.0f;
vIntersect = new MCVector3();
// 平面交差点の間隔を得る:
// 球体位置から平面までの距離を計算します。
float signedDistToPlane = DistanceTo(vA);
//
float normalDotVelocity = vNormal.Dot(vVel);
// 球体が平面に平行をになっているか？:
if (normalDotVelocity == 0.0f)
{
if (System.Math.Abs(signedDistToPlane) >= fEpsilon)
{
// 厚みfEpsilon内の平面に埋め込まれていない
// 衝突してない
return(0);
}
// 球体は平面に埋め込まれています。
time = 0.0f;
vIntersect = GetClosestPtPoint(vA);
return(2);
}
else
{
// 平面と交差する方向のある直線abと交差するtの値を計算
time = (distance - vNormal.Dot(vA)) / vNormal.Dot(vVel);
// tが[0..1]の中にある場合、交差点を計算して返す
if (time >= 0.0f && time <= 1.0f)
{
vIntersect = vA + (vVel * time);
return(1);
}
// そうでない場合tは+INF, -INF, NaN, あるいは[0..1]の中にはないので、交差なし
}
return(0);
}
/// <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>
/// 動いている球体の当たり判定
/// </summary>
/// <param name="v0">この(s0)球体のベロシティー</param>
/// <param name="s1">球体１</param>
/// <param name="v1">s1の球体のベロシティー</param>
/// <param name="t">時間</param>
/// <returns>静止している球 s1 に対して交差している場合はtrueを返し、衝突の時間*tも返す</returns>
public bool MovingSphereSphere(MCVector3 v0, Sphere s1, MCVector3 v1, out float t)
{
t = 0;
// 球s1をs0の半径にまで拡張
Sphere S1Ex = s1;
S1Ex.r += this.r;
// s0およびs1の両方からs1の運動を引き算し、s1を静止させる
MCVector3 v = v0 - v1;
// これで、方向のある線分 s = s0.c + tv, v = (v0-v1)/||v0-v1|| を
// 拡張した球に対して交差させることができる
MCVector3 vQ;
float fVLen = v.Length();
v /= fVLen;
if (S1Ex.IntersectRaySphere(this.c, v, out t, out vQ))
{
return(t <= fVLen);
}
return(false);
}
/// <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);
}
