/// <summary>
/// カプセル衝突判定
/// </summary>
/// <param name="nextpos0">エッジの始点</param>
/// <param name="nextpos1">エッジの終点</param>
/// <param name="corr0"></param>
/// <param name="corr1"></param>
/// <param name="radius"></param>
/// <param name="cindex"></param>
/// <param name="dir"></param>
/// <returns></returns>
bool CapsuleColliderDetection(float3 nextpos0, float3 nextpos1, ref float3 corr0, ref float3 corr1, float radius, int cindex, float3 dir)
//bool CapsuleColliderDetection(float3 nextpos0, float3 nextpos1, float3 oldpos0, float3 oldpos1, ref float3 corr0, ref float3 corr1, float radius, int cindex, float3 dir)
{
var cpos = nextPosList[cindex];
var crot = nextRotList[cindex];
var coldpos = posList[cindex];
var coldrot = rotList[cindex];
// x = 長さ（片側）
// y = 始点半径
// z = 終点半径
//var lpos = localPosList[cindex];
var cradius = radiusList[cindex];
// スケール
var tindex = transformIndexList[cindex];
var cscl = boneSclList[tindex];
float scl = math.dot(cscl, dir); // dirの軸のスケールを使用する
cradius *= scl;
// 移動前のコライダーに対するエッジの最近接点を求める
float3 oldl = math.mul(coldrot, dir * cradius.x);
float3 soldpos = coldpos - oldl;
float3 eoldpos = coldpos + oldl;
float3 c1, c2;
float s, t;
MathUtility.ClosestPtSegmentSegment(soldpos, eoldpos, nextpos0, nextpos1, out s, out t, out c1, out c2);
//MathUtility.ClosestPtSegmentSegment(soldpos, eoldpos, oldpos0, oldpos1, out s, out t, out c1, out c2);
float3 v = c2 - c1;
// 現在のカプセル始点と終点
float3 l = math.mul(crot, dir * cradius.x);
float3 spos = cpos - l;
float3 epos = cpos + l;
float sr = cradius.y;
float er = cradius.z;
// 移動後のコライダーのベクトルに変換する
var iq = math.inverse(coldrot);
float3 lv = math.mul(iq, v);
v = math.mul(crot, lv);
// コライダーの半径
float r = math.lerp(sr, er, s);
// 平面方程式
float3 n = math.normalize(v);
float3 q = math.lerp(spos, epos, s);
float3 c = q + n * (r + radius);
// c = 平面位置
// n = 平面方向
// 平面衝突判定と押し出し
float3 outpos0, outpos1;
bool ret0 = MathUtility.IntersectPointPlane(c, n, nextpos0, out outpos0);
bool ret1 = MathUtility.IntersectPointPlane(c, n, nextpos1, out outpos1);
if (ret0)
{
corr0 += outpos0 - nextpos0;
}
if (ret1)
{
corr1 += outpos1 - nextpos1;
}
return(ret0 || ret1);
}