public void Reset()
{
convexA = null;
convexB = null;
edgeNodeA = null;
edgeNodeB = null;
penetratedNode = null;
depth = 1e+8f;
}
private void Start()
{
if (simulator && convexData)
{
Convex convex = Convex.LoadFromConvexData(convexData, transform.position, transform.eulerAngles.z * Mathf.Deg2Rad, transform.localScale);
_mine = simulator.AddConvex(convex, true, true);
// _mine = simulator.AddTriangle(transform.position, size);
}
}
public static Convex LoadFromConvexData(SBPConvexData data, Vector2 translation, float rotation, Vector2 scale)
{
if (data == null)
{
return(null);
}
// ノードを作成
List <Node> nodes = new List <Node>();
for (int i = 0; i < data.nodes.Length; ++i)
{
SBPNodeData nodeData = data.nodes[i];
Vector2 position = nodeData.position;
position *= scale;
position = Utilities.RotateVector(position, rotation);
position += translation;
Node n = new Node(position, nodeData.mass, nodeData.damping);
nodes.Add(n);
}
// エッジを作成
List <Edge> edges = new List <Edge>();
for (int i = 0; i < data.edges.Length; ++i)
{
SBPEdgeData edgeData = data.edges[i];
Edge e = new Edge(nodes[edgeData.aIdx], nodes[edgeData.bIdx]);
edges.Add(e);
}
// 凸包を作成
Convex convex = new Convex();
int offset = data.helperNodeOffset;
for (int i = 0; i < nodes.Count; ++i)
{
if (i < offset)
{
convex.AddCollisionNode(nodes[i]);
}
else
{
convex.AddHelperNode(nodes[i]);
}
}
convex.edges = edges;
convex.RecalculateBounds();
return(convex);
}
private void DrawConvexNormalsOnGizmos()
{
for (int k = 0; k < _convexes.Count; ++k)
{
Convex c = _convexes[k];
for (int i = 0, j = c.collisionNodes.Count - 1; i < c.collisionNodes.Count; j = i++)
{
Node a = c.collisionNodes[j];
Node b = c.collisionNodes[i];
Vector2 mid = (a.position + b.position) * 0.5f;
Vector2 normal = new Vector2(b.position.y - a.position.y, a.position.x - b.position.x).normalized;
Gizmos.DrawLine(mid, mid + normal);
}
}
}
/// <summary>
/// 二凸包間の衝突判定
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="info"></param>
void CheckNarrowPhaseCollision2(Convex a, Convex b)
{
// 2つの凸包が衝突しているかの少し粗い確認
if (a.aabb.max.y < b.aabb.min.y ||
a.aabb.min.y > b.aabb.max.y ||
a.aabb.max.x < b.aabb.min.x ||
a.aabb.min.x > b.aabb.max.x ||
(a.isStatic && b.isStatic))
{
return;
}
// ２つの凸包に対して分離線が定義出来るかの確認
if (CheckCollisionWithSAT(a, b, ref satResultAB) && CheckCollisionWithSAT(b, a, ref satResultBA))
{
// ２つの凸包間でそれぞれ分離軸が定義できないので、２つの凸包は衝突している。
CollisionInfo info = GetCollisionInfo();
if (satResultAB.penetration < satResultBA.penetration)
{
// FindShallowestPenetratedNode(b, ref satResultAB);
FindDeepestPenetratedNode(b, ref satResultAB);
info.convexA = a;
info.convexB = b;
info.edgeNodeA = satResultAB.edgeNodeA;
info.edgeNodeB = satResultAB.edgeNodeB;
info.penetratedNode = satResultAB.penetratedNode;
info.depth = satResultAB.penetration;
info.axis = satResultAB.axis;
}
else
{
// FindShallowestPenetratedNode(a, ref satResultBA);
FindDeepestPenetratedNode(a, ref satResultBA);
info.convexA = b;
info.convexB = a;
info.edgeNodeA = satResultBA.edgeNodeA;
info.edgeNodeB = satResultBA.edgeNodeB;
info.penetratedNode = satResultBA.penetratedNode;
info.depth = satResultBA.penetration;
info.axis = satResultBA.axis;
}
_collisionInfoList.Add(info);
CollisionResponse(info);
}
}
/// <summary>
/// SAT(分離軸定理)による衝突の検出
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="info"></param>
bool CheckCollisionWithSAT(Convex a, Convex b, ref SATResult result)
{
// ２つの凸包を分離する分離線が存在する場合、それは２つの凸包のいずれかの辺に平行である。
// これが凸包同士の分離軸定理なので、これをもとに衝突判定を行い、それを解決するための情報を集める。
// そのためにまずは最も重なりの少ない分離軸を探索する。
// その後、その分離軸に対して最も深くめり込んでいる頂点を処理する。
result.penetration = LARGE;
result.edgeNodeA = null;
result.edgeNodeB = null;
for (int i = 0, j = a.collisionNodes.Count - 1; i < a.collisionNodes.Count; j = i++)
{
Node edgeNodeA = a.collisionNodes[j];
Node edgeNodeB = a.collisionNodes[i];
Vector2 pa = edgeNodeA.position;
Vector2 pb = edgeNodeB.position;
Vector2 axis = new Vector2(pb.y - pa.y, pa.x - pb.x).normalized;
float aMin, aMax, bMin, bMax, signedAmount;
ProjectConvexToAxis(a, axis, out aMin, out aMax);
ProjectConvexToAxis(b, axis, out bMin, out bMax);
MeasureOverlappedSignedAmount(aMin, aMax, bMin, bMax, out signedAmount);
if (signedAmount <= 0f)
{
// 重なりが存在しない場合はその範囲に分離線が定義できてしまうので、衝突しない。
return(false);
}
if (signedAmount < result.penetration)
{
// 重なりが最も小さい場合はその量と辺を構成するノードを記録する。
result.penetration = signedAmount;
result.edgeNodeA = edgeNodeA;
result.edgeNodeB = edgeNodeB;
result.axis = axis;
}
}
return(true);
}
/// <summary>
/// 凸包同士の衝突判定のブロードフェイズ
/// x軸に従ってソートされているはずなのでその特定を活かす。
/// </summary>
void CheckBroadPhaseCollision()
{
for (int i = 0; i < _convexes.Count; ++i)
{
for (int j = i + 1; j < _convexes.Count; ++j)
{
Convex a = _convexes[i];
Convex b = _convexes[j];
if (a.aabb.max.x < b.aabb.min.x)
{
// 事前にソートしているはずなので、これ以降は衝突しない。
break;
}
CheckNarrowPhaseCollision2(a, b);
}
}
}
/// <summary>
/// 最も浅いノードを返す。
/// </summary>
/// <param name="convex"></param>
/// <param name="result"></param>
void FindShallowestPenetratedNode(Convex convex, ref SATResult result)
{
Vector2 pa = result.edgeNodeA.position;
Vector2 pb = result.edgeNodeB.position;
float minDistance = LARGE;
Node bestNode = null;
for (int i = 0; i < convex.collisionNodes.Count; ++i)
{
Node n = convex.collisionNodes[i];
float distance = Utilities.DistanceToSegment(n.position, pa, pb);
if (distance < minDistance)
{
minDistance = distance;
bestNode = n;
}
}
result.penetratedNode = bestNode;
}
/// <summary>
/// 凸包同士の衝突を判定する。
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
void CheckNarrowPhaseCollision(Convex a, Convex b)
{
// 2つの凸包について細かく判定
if (a.aabb.max.y < b.aabb.min.y ||
a.aabb.min.y > b.aabb.max.y ||
a.aabb.max.x < b.aabb.min.x ||
a.aabb.min.x > b.aabb.max.x ||
(a.isStatic && b.isStatic))
{
return;
}
CollisionInfo info = GetCollisionInfo();
if (CollideConvexes(a, b, ref info) && CollideConvexes(b, a, ref info))
{
CollisionResponse(info);
_collisionInfoList.Add(info);
}
}
/// <summary>
/// 軸(axis)に凸包(convex)を射影したときの、最小値(min)と最大値(max)を求める。
/// </summary>
/// <param name="convex"></param>
/// <param name="axis"></param>
/// <param name="min"></param>
/// <param name="max"></param>
void ProjectConvexToAxis(Convex convex, Vector2 axis, out float min, out float max)
{
Node n = convex.collisionNodes[0];
min = max = Vector2.Dot(axis, n.position);
for (int i = 1; i < convex.collisionNodes.Count; ++i)
{
n = convex.collisionNodes[i];
float t = Vector2.Dot(axis, n.position);
if (t < min)
{
min = t;
}
else if (t > max)
{
max = t;
}
}
}
/// <summary>
/// 最も深くめり込んでいるノードを返す。
/// </summary>
/// <param name="convex"></param>
/// <param name="result"></param>
void FindDeepestPenetratedNode(Convex convex, ref SATResult result)
{
Vector2 pa = result.edgeNodeA.position;
Vector2 pb = result.edgeNodeB.position;
Vector2 mid = (pa + pb) * 0.5f;
float minDistance = LARGE;
Node bestNode = null;
for (int i = 0; i < convex.collisionNodes.Count; ++i)
{
// 軸方向に射影した場合に、最も小さいものが一番めり込んでいる。
Node n = convex.collisionNodes[i];
float distance = Vector2.Dot(n.position - mid, result.axis);
if (distance < minDistance)
{
minDistance = distance;
bestNode = n;
}
}
result.penetratedNode = bestNode;
}
public Convex AddConvex(Convex c, bool addNodes, bool addEdges)
{
_convexes.Add(c);
if (addNodes)
{
for (int i = 0; i < c.collisionNodes.Count; ++i)
{
AddNode(c.collisionNodes[i]);
}
for (int i = 0; i < c.helperNodes.Count; ++i)
{
AddNode(c.helperNodes[i]);
}
}
if (addEdges)
{
for (int i = 0; i < c.edges.Count; ++i)
{
AddEdge(c.edges[i]);
}
}
return(c);
}
/// <summary>
/// 凸包同士の衝突判定
/// 衝突判定には分離軸定理を用いる。
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
bool CollideConvexes(Convex a, Convex b, ref CollisionInfo info)
{
// aのエッジにbのノードが挿入されているかどうかを判定する。
info.convexA = a;
info.convexB = b;
float minDepth = info.depth;
Vector2 collisionNormal = new Vector2();
bool foundBestNormal = false;
Node edgeA = null, edgeB = null;
// 衝突時のノーマルとその深さを取得する。
for (int i = 0, j = a.collisionNodes.Count - 1; i < a.collisionNodes.Count; j = i++)
{
Node n1 = a.collisionNodes[i];
Node n2 = a.collisionNodes[j];
Vector2 p1 = n1.position;
Vector2 p2 = n2.position;
// 面の法線
// 2点間の法線を求める賢い方法やね。
// Vector2 normal = new Vector2(p1.y - p2.y, p2.x - p1.x).normalized;
Vector2 normal = new Vector2(p2.y - p1.y, p1.x - p2.x).normalized;
// Vector2 normal = Utilities.RotateVector(p1 - p2, 90f * Mathf.Deg2Rad).normalized;
// 法線に対して2つの凸包の頂点を射影し、その最大値と最小値を求める。
// Aの最大値とBの最小値、またはAの最小値とBの最大値の符号付き距離(signed distance)を計算する。
// 距離が負の場合はその辺n1,n2に対してその距離の絶対値だけめり込みが起きていることになる。
float aMin, aMax, bMin, bMax, signedAmount;
ProjectConvexToAxis(a, normal, out aMin, out aMax);
ProjectConvexToAxis(b, normal, out bMin, out bMax);
MeasureOverlappedSignedAmount(aMin, aMax, bMin, bMax, out signedAmount);
// 中心座標間の差分ベクトルと法線との内積を求める。
Vector2 centersDirection = a.aabb.center - b.aabb.center;
float normalResponceProjection = Vector2.Dot(centersDirection, normal);
if (signedAmount <= 0)
{
// 分離軸に射影した点が交差していない場合は、2つの凸法を分離するための軸が必ず存在する。
return(false);
}
else if (normalResponceProjection > 0 && signedAmount < minDepth)
{
// 法線と中心間の差分ベクトルとの向きが90度未満で、
// めり込み距離が現在記録しているものよりも小さい場合。
minDepth = Mathf.Abs(signedAmount);
collisionNormal = normal;
foundBestNormal = true;
edgeA = n2;
edgeB = n1;
}
}
if (foundBestNormal)
{
float maxDistance = 1e+5f;
Node firstNodeOnCorrectSide = null;
for (int i = 0; i < b.collisionNodes.Count; i++)
{
// もう一つの凸包の頂点と選択された辺との位置関係を確認する。
if (Utilities.IsClockwise(b.collisionNodes[i].position, edgeA.position, edgeB.position))
{
// 時計回りに位置する場合は無視
continue;
}
// エッジからの距離を求める。
float distanceToEdgeFromPoint = Utilities.DistanceToSegment(b.collisionNodes[i].position, edgeA.position, edgeB.position);
if (distanceToEdgeFromPoint < maxDistance)
{
maxDistance = distanceToEdgeFromPoint;
info.penetratedNode = b.collisionNodes[i];
}
firstNodeOnCorrectSide = b.collisionNodes[i];
}
if (info.penetratedNode == null)
{
info.penetratedNode = firstNodeOnCorrectSide != null ? firstNodeOnCorrectSide : b.collisionNodes[0];
}
info.axis = collisionNormal;
info.edgeNodeA = edgeA;
info.edgeNodeB = edgeB;
info.depth = minDepth;
}
return(true);
}
