/// <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>
/// <returns></returns>
bool SphereColliderDetection(float3 nextpos0, float3 nextpos1, ref float3 corr0, ref float3 corr1, float radius, int cindex)
//bool SphereColliderDetection(float3 nextpos0, float3 nextpos1, float3 oldpos0, float3 oldpos1, ref float3 corr0, ref float3 corr1, float radius, int cindex)
{
var cpos = nextPosList[cindex];
var coldpos = posList[cindex];
var cradius = radiusList[cindex];
// コライダー球とエッジの最接近点を求める
float3 d = MathUtility.ClosestPtPointSegment(coldpos, nextpos0, nextpos1);
//float3 d = MathUtility.ClosestPtPointSegment(coldpos, oldpos0, oldpos1);
float3 n = math.normalize(d - coldpos);
float3 c = cpos + n * (cradius + 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);
}
/// <summary>
/// 平面制限
/// </summary>
/// <param name="data"></param>
/// <param name="cindex"></param>
/// <param name="nextpos"></param>
/// <param name="outpos"></param>
/// <returns></returns>
private bool PlanePenetration(ref PenetrationData data, float teamScale, float distance, int cindex, float3 nextpos, out float3 outpos)
{
var cpos = nextPosList[cindex];
var crot = nextRotList[cindex];
// スケール
var tindex = transformIndexList[cindex];
var cscl = boneSclList[tindex];
// 中心軸
var d = math.mul(crot, data.localPos * cscl) + cpos;
// 方向
var n = math.mul(crot, data.localDir);
// 押し出し平面を求める
var c = d + n * (data.distance * teamScale - distance);
// c = 平面位置
// n = 平面方向
// 平面衝突判定と押し出し
return(MathUtility.IntersectPointPlane(c, n, nextpos, out outpos));
}
//public NativeArray<float> frictionList;
// パーティクルごと
public void Execute(int index)
{
// 初期化コピー
float3 nextpos = nextPosList[index];
outNextPosList[index] = nextpos;
var flag = flagList[index];
if (flag.IsValid() == false || flag.IsFixed() || flag.IsCollider())
{
return;
}
// チーム
var team = teamIdList[index];
var teamData = teamDataList[team];
if (teamData.IsActive() == false)
{
return;
}
if (teamData.penetrationGroupIndex < 0)
{
return;
}
// 更新確認
if (teamData.IsUpdate() == false)
{
return;
}
// グループデータ
var gdata = groupList[teamData.penetrationGroupIndex];
if (gdata.active == 0)
{
return;
}
// データ参照情報
int vindex = index - teamData.particleChunk.startIndex;
var refdata = refDataList[gdata.refDataChunk.startIndex + vindex];
if (refdata.count == 0)
{
return;
}
// depth
var depth = depthList[index];
// move radius
var moveradius = gdata.radius.Evaluate(depth);
// 浸透距離
float distance = gdata.distance.Evaluate(depth);
// チームスケール倍率
float3 scaleDirection = teamData.scaleDirection;
float teamScale = teamData.scaleRatio;
distance *= teamScale;
moveradius *= teamScale;
// モード別処理
var oldpos = nextpos;
if (gdata.mode == 0)
{
// Surface Penetration
// ベース位置から算出する
var bpos = basePosList[index];
var brot = baseRotList[index];
int dindex = refdata.startIndex;
var data = dataList[gdata.dataChunk.startIndex + dindex];
if (data.IsValid())
{
//float3 n = math.mul(brot, data.localDir);
float3 n = math.mul(brot, data.localDir * scaleDirection); // マイナススケール対応
// 球の位置
var c = bpos + n * (distance - moveradius);
// 球内部制限
var v = nextpos - c;
var len = math.length(v);
if (len > moveradius)
{
v *= (moveradius / len);
nextpos = c + v;
}
}
}
else if (gdata.mode == 1)
{
// Collider Penetration
#if true
// 球内制限
float3 c = 0;
int ccnt = 0;
int dindex = refdata.startIndex;
for (int i = 0; i < refdata.count; i++, dindex++)
{
var data = dataList[gdata.dataChunk.startIndex + dindex];
if (data.IsValid())
{
int cindex = colliderList[teamData.colliderChunk.startIndex + data.colliderIndex];
var cflag = flagList[cindex];
if (cflag.IsValid() == false)
{
continue;
}
// 球内部制限
c += InverseSpherePosition(ref data, teamScale, scaleDirection, distance, cindex, moveradius);
ccnt++;
}
}
if (ccnt > 0)
{
c /= ccnt;
var opos = InverseSpherePenetration(c, moveradius, nextpos);
var addv = (opos - nextpos);
// stiffness test
//addv *= 0.25f;
// 摩擦を入れてみる
//float friction = math.length(addv) * 10.0f;
//frictionList[index] = math.max(friction, frictionList[index]); // 大きい方
nextpos += addv;
}
#else
// 平面制限
float3 c = 0;
float3 n = 0;
int ccnt = 0;
int dindex = refdata.startIndex;
for (int i = 0; i < refdata.count; i++, dindex++)
{
var data = dataList[gdata.dataChunk.startIndex + dindex];
if (data.IsValid())
{
int cindex = colliderList[teamData.colliderChunk.startIndex + data.colliderIndex];
// プレーン制限（今のところあまり有効性なし）
float3 center, dir;
InversePlanePosition(ref data, teamScale, distance, cindex, out center, out dir);
c += center;
n += dir;
ccnt++;
}
}
if (ccnt > 0)
{
c /= ccnt;
n = math.normalize(n);
// c = 平面位置
// n = 平面方向
// 平面衝突判定と押し出し
float3 opos;
MathUtility.IntersectPointPlane(c, n, nextpos, out opos);
var addv = (opos - nextpos);
// stiffness test
//addv *= 0.25f;
nextpos += addv;
}
#endif
}
// 速度影響はなし
//posList[index] += (nextpos - oldpos); // 取りやめ、影響は100%にする(v1.8.0)
// 書き戻し
outNextPosList[index] = nextpos;
}
/// <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);
}