private Convex(Space space, ConvexData data)
: base(NativeMethods.dCreateConvex(space != null ? space.Id : dSpaceID.Null,
data._planes, data._planeCount,
data._points, data._pointCount,
data._polygons))
{
this.data = data;
}
void OnGUI()
{
GUILayout.BeginVertical();
cc = EditorGUILayout.ObjectField("凸包文件：", cc, typeof(ConvexCollection)) as ConvexCollection;
if (GUILayout.Button("创建Convexdatas"))
{
cc = ScriptableObject.CreateInstance <ConvexCollection>();
AssetDatabase.CreateAsset(cc, "Assets/Resources/ConvexDataCollection.asset");
AssetDatabase.SaveAssets();
}
if (GUILayout.Button("创建Convex"))
{
GameObject cur = Selection.activeGameObject;
if (cur != null)
{
Mesh m = cur.GetComponent <MeshFilter>().sharedMesh;
if (m != null)
{
ConvexData cd = new ConvexData();
if (ConvexUtil.MakeHull(m, cur.transform, "", ref cd))
{
cc.ConvexDatas.Add(cd);
}
}
}
}
if (GUILayout.Button("从文件添加凸包"))
{
LEditTextFile tf = new LEditTextFile();
tf.OpenRead("convex.txt");
ConvexData cd = new ConvexData();
cd.EditLoad(tf);
cc.ConvexDatas.Add(cd);
tf.Close();
}
if (GUILayout.Button("显示凸包"))
{
cc.DebugRender(debugShow);
debugShow = !debugShow;
}
if (GUILayout.Button("保存"))
{
AssetDatabase.DeleteAsset("Assets/Resources/ConvexDataCollection.asset");
AssetDatabase.CreateAsset(cc, "Assets/Resources/ConvexDataCollection.asset");
AssetDatabase.SaveAssets();
}
}
/// <summary>
/// Sets the convex hull data.
/// </summary>
/// <param name="planes">
/// The planes' definition array. Each polygon in the convex hull will be
/// attached to one of these planes. Each plane is defined by a quadruplet
/// of the a, b, c and d parameters of the plane equation.
/// </param>
/// <param name="points">
/// The array of points, aligned in triplets of x, y and z components.
/// </param>
/// <param name="polygons">
/// The polygons' definition array. Each element in the array is an index
/// to a previously defined point in the array of point triplets.
/// Each polygon definition sequence begins and ends in the same point.
/// There must be as many polygon definition sequences as the number of planes.
/// </param>
public void SetConvex(dReal[] planes, dReal[] points, int[] polygons)
{
if (data != null)
{
data.Dispose();
}
data = new ConvexData(planes, points, polygons);
NativeMethods.dGeomSetConvex(Id, data._planes, data._planeCount,
data._points, data._pointCount,
data._polygons);
}
/// <summary>
/// Releases the unmanaged resources used by the <see cref="Convex"/> class
/// specifying whether to perform a normal dispose operation.
/// </summary>
/// <param name="disposing">
/// <b>true</b> for a normal dispose operation; <b>false</b> to finalize
/// the geom.
/// </param>
protected override void Dispose(bool disposing)
{
if (data != null)
{
if (disposing)
{
data.Dispose();
data = null;
}
}
base.Dispose(disposing);
}
public void Set(ConvexData src)
{
Reset();
int i;
for (i = 0; i < src.GetVertexNum(); i++)
{
AddVertex(src.mLstVertices[i]);
}
for (i = 0; i < src.GetFaceNum(); i++)
{
AddFace(src.mLstCovFace[i]);
}
Flags = src.Flags;
mAABB = src.mAABB;
}
//导出数据到ConvexData中
public void ExportCHData(ConvexData chData)
{
List <int> lstMap = new List <int>(); //一个由顶点在本类中id到CConvexHullData中id的一个映射表；
//遍历每一个Patch
for (int i = 0; i < LstPatches.Count; i++)
{
Patch pat = LstPatches[i];
Vector3 n = pat.Normal;
CovFace face = new CovFace();
face.Normal = n;
face.Dist = pat.Dist;
for (int j = 0; j < pat.GetVNum(); j++)
{
int vid = pat.GetVID(j);
//构造垂直于该边法向朝外的HalfSpace
Vector3 v1 = Vector3.zero;
Vector3 v2 = Vector3.zero;
pat.GetEdge(j, ref v1, ref v2);
int ExistID = FindInArray(vid, lstMap);
if (ExistID == -1)
{
//说明是新的顶点
//插入到CConvexHullData的Vertices中
chData.AddVertex(v1);
int newID = chData.GetVertexNum() - 1; //在pCHData中的id
face.AddElement(newID);
lstMap.Add(vid);
}
else
{
//说明是已经存在的顶点
face.AddElement(ExistID);
}
}
//插入该面片
chData.AddFace(face);
}
}
static Vector3 getClosestPoint(Vector3 closest, int size, ConvexData convex)
{
Vector3 closestB = Vector3.zero;
switch (size)
{
case 1:
{
closestB = convex.GetVertex(B[0]);
break;
}
case 2:
{
float v = 0;
Vector3 B0 = convex.GetVertex(B[0]);
Vector3 B1 = convex.GetVertex(B[1]);
barycentricCoordinates(closest, Q[0], Q[1], ref v);
Vector3 bv = B1 - B0;
closestB = bv * v + B0;
break;
}
case 3:
{
//calculate the Barycentric of closest point p in the mincowsky sum
float v = 0, w = 0;
Vector3 B0 = convex.GetVertex(B[0]);
Vector3 B1 = convex.GetVertex(B[1]);
Vector3 B2 = convex.GetVertex(B[2]);
barycentricCoordinates(closest, Q[0], Q[1], Q[2], ref v, ref w);
Vector3 bv0 = B1 - B0;
Vector3 bv1 = B2 - B0;
closestB = B0 + bv0 * v + bv1 * w;
break;
}
}
return(closestB);
}
public ConvexData(ConvexData chData)
{
Set(chData);
}
public static bool MakeHull(Mesh mesh, Transform tr, string path, ref ConvexData data)
{
GiftWrap gw = new GiftWrap();
gw.Reset();
List <Vector3> vecVertex = new List <Vector3>();
Vector3 vmin = Vector3.zero;
Vector3 vmax = Vector3.zero;
Bounds bound = mesh.bounds;
vmin = bound.min;
vmax = bound.max;
Vector3[] verts = mesh.vertices;
for (int j = 0; j < mesh.vertexCount; ++j)
{
Vector3 wp = verts[j];
if (null != tr)
{
wp = tr.TransformPoint(wp);
}
bool bnear = false;
for (int k = 0; k < (int)vecVertex.Count; k++)
{
if ((wp - vecVertex[k]).magnitude < CLOSE_VERTEX_DISTANCE_THRESH)
{
bnear = true;
break;
}
}
if (!bnear)
{
vecVertex.Add(wp);
}
}
int numallvert = vecVertex.Count;
gw.SetVertexes(vecVertex);
gw.ComputeConvexHull();
path += "/" + System.DateTime.Now.ToString("dd-MM-yy-HH-mm-ss") + "_vts.txt";
if (gw.ExceptionOccur())
{
gw.SaveVerticesToFile(path);
gw.Reset();
return(false);
}
ConvexPolytope cp = new ConvexPolytope();
cp.Init(gw, (vmax - vmin).magnitude);
if (cp.ExceptionOccur && cp.MinPatchNum > 20)
{
gw.SaveVerticesToFile(path);
return(false);
}
int patchnum = Mathf.Min(cp.OriginPatchNum, Mathf.Max(10, cp.MinPatchNum));
if (patchnum > MAX_FACE_IN_HULL)
{
gw.Reset();
return(false);
}
else
{
cp.Goto(Mathf.Min(patchnum, MAX_FACE_IN_HULL));
}
gw.Reset();
data.Reset();
cp.ExportCHData(data);
return(true);
}
static float GJK_RELATIVE_EPSILON = 0.0004f; //square of 2%.
//ML: if we are using gjk local which means one of the object will be sphere/capsule, in that case, if we define takeCoreShape is true, we just need to return the closest point as the sphere center or a point in the capsule segment. This will increase the stability
//for the manifold recycling code
public static GJKType GjkLocalPenetration_CapsuleConvex(CAPSULE a, ConvexData b, ref Vector3 normal, ref float penetrationDepth, ref CollidePoints points, ref Vector3 closeB)
{
float marginA = a.getMargin();
float marginB = 0; //b.getMargin();
//float minMargin = 0;// Mathf.Min(a.getMinMargin(), b.getMinMargin());
//float _eps2 = (minMargin * (0.1f));
//ML: eps2 is the threshold that uses to detemine whether two (shrunk) shapes overlap. We calculate eps2 based on 10% of the minimum margin of two shapes
float eps2 = 0; // (_eps2 * _eps2);
//ML: epsRel2 is the square of 1.5%. This is used to scale the the sqaure distance of a closet point to origin to detemine whether two shrunk shapes overlap in the margin, but
//they don't overlap.
float epsRel2 = GJK_RELATIVE_EPSILON;
float sumOrignalMargin = marginA + marginB;
float sDist = float.MaxValue;
float minDist = sDist;
bool bNotTerminated = true;
bool bCon = true;
Vector3 closest;
Vector3 supportA = Vector3.zero, supportB = Vector3.zero, support = Vector3.zero;
int size = 0;
Vector3 _initialSearchDir = a.Center - b.GetAABB().center;
closest = Vector3.Dot(_initialSearchDir, _initialSearchDir) > 0 ? _initialSearchDir : Vector3.right;
Vector3 prevClosest = closest;
int bIndex = 0;
// ML : termination condition
//(1)two (shrunk)shapes overlap. GJK will terminate based on sq(v) < eps2 and indicate that two shapes are overlapping.
//(2)two (shrunk + margin)shapes separate. If sq(vw) > sqMargin * sq(v), which means the original objects do not intesect, GJK terminate with GJK_NON_INTERSECT.
//(3)two (shrunk) shapes don't overlap. However, they interect within margin distance. if sq(v)- vw < epsRel2*sq(v), this means the shrunk shapes interect in the margin,
// GJK terminate with GJK_CONTACT.
while (bNotTerminated)
{
//minDist, tempClosA, tempClosB are used to store the previous iteration's closest points(in A and B space) and the square distance from the closest point
//to origin in Mincowski space
minDist = sDist;
prevClosest = closest;
supportA = a.supportSweepLocal(-closest);
supportB = b.supportSweepLocal(closest, ref bIndex);
//calculate the support point
support = supportA - supportB;
Q[size] = support;
B[size] = bIndex;
//ML: because we shrink the shapes by plane shifting(box and convexhull), the distance from the "shrunk" vertices to the original vertices may be larger than contact distance.
//therefore, we need to take the largest of these 2 values into account so that we don't incorrectly declare shapes to be disjoint. If we don't do this, there is
//an inherent inconsistency between fallback SAT tests and GJK tests that may result in popping due to SAT discovering deep penetrations that were not detected by
//GJK operating on a shrunk shape.
float sqMargin = (sumOrignalMargin * sumOrignalMargin);
float vw = Vector3.Dot(closest, support);
bool con = (vw > 0) && (vw * vw > sDist * sqMargin); //this is the non intersect condition
bool conGrtr = (epsRel2 * sDist) >= (sDist - vw); //this is the margin intersect condition
bool conOrconGrtr = con || conGrtr;
if (conOrconGrtr)
{
if (!con) //must be true otherwise we wouldn't be in here...
{
float dist = Mathf.Sqrt(sDist);
//PX_ASSERT(FAllGrtr(dist, FEps));
Vector3 n = closest / dist; //normalise
normal = n;
closeB = getClosestPoint(closest, size, b);
penetrationDepth = dist - sumOrignalMargin;
points.size = size;
points.a = B[0];
points.b = B[1];
points.c = B[2];
return(GJKType.GJK_CONTACT);
}
else
{
return(GJKType.GJK_NON_INTERSECT);
}
}
size++;
//PX_ASSERT(size <= 4);
//calculate the closest point between two convex hull
closest = GJKCPairDoSimplex(support, ref size);
sDist = Vector3.Dot(closest, closest);
bCon = minDist > sDist;
bNotTerminated = sDist > eps2 && bCon;
}
if (!bCon)
{
sDist = minDist;
float sqExpandedMargin = sumOrignalMargin * sumOrignalMargin;
//Reset back to older closest point
closest = prevClosest;
closeB = getClosestPoint(closest, size, b);
sDist = minDist;
float dist = Mathf.Sqrt(sDist);
//PX_ASSERT(dist > FEps);
Vector3 n = closest / dist; //normalise
penetrationDepth = dist - sumOrignalMargin;
normal = n;
points.size = size;
points.a = B[0];
points.b = B[1];
points.c = B[2];
if (sqExpandedMargin >= sDist)
{
return(GJKType.GJK_CONTACT);
}
//此时明明没有碰撞，但是误差导致的碰撞，向法线方向移动0.001f
//penetrationDepth = -0.001f;
return(GJKType.GJK_DEGENERATE);
}
else
{
//this two shapes are deeply intersected with each other, we need to use EPA algorithm to calculate MTD
return(GJKType.EPA_CONTACT);
}
}
static public bool GjkLocalRayCast_CapsuleConvex(CAPSULE a, ConvexData b, Vector3 r, ref float lambda, ref Vector3 normal, ref bool StartSolid, ref CollidePoints points, ref Vector3 closeB)
{
if (b.GetVertexNum() == 0)
{
return(false);
}
bool _StartSolid = true;
float inflation = a.Radius;
float maxDist = float.MaxValue;
float _lambda = 0;
r = -r;
Vector3 x = r * _lambda;
int size = 1;
Vector3 dir = a.Center - b.GetAABB().center;
Vector3 _initialSearchDir = (Vector3.Dot(dir, dir) > FEps) ? dir : Vector3.right;
Vector3 initialSearchDir = Vector3.Normalize(_initialSearchDir);
int bIndex = 0;
Vector3 initialSupportA = a.supportSweepLocal(-initialSearchDir);
Vector3 initialSupportB = b.supportSweepLocal(initialSearchDir, ref bIndex);
Q[0] = initialSupportA - initialSupportB; Q[1] = Vector3.zero; Q[2] = Vector3.zero; Q[3] = Vector3.zero; //simplex set
B[0] = bIndex; B[1] = 0; B[2] = 0; B[3] = 0; //ConvexHull b simplex set
Vector3 closest = Q[0];
Vector3 supportA = initialSupportA;
Vector3 supportB = initialSupportB;
Vector3 support = Q[0];
//float minMargin = Mathf.Min(a.getSweepMargin(), b.getSweepMargin());
float eps1 = 0;//minMargin * 0.1f;
float inflationPlusEps = eps1 + inflation;
float eps2 = eps1 * eps1;
float inflation2 = inflationPlusEps * inflationPlusEps;
float sDist = Vector3.Dot(closest, closest);
float minDist = sDist;
bool bNotTerminated = sDist > eps2;
bool bCon = true;
Vector3 nor = closest;
Vector3 prevClosest = closest;
while (bNotTerminated == true)
{
minDist = sDist;
prevClosest = closest;
Vector3 vNorm = -Vector3.Normalize(closest);
supportA = a.supportSweepLocal(vNorm);
supportB = x + b.supportSweepLocal(-vNorm, ref bIndex);
//calculate the support point
support = supportA - supportB;
Vector3 w = -support;
float vw = Vector3.Dot(vNorm, w) - inflationPlusEps;
float vr = Vector3.Dot(vNorm, r);
if (vw > 0)
{
if (vr >= 0)
{
return(false);
}
else
{
_StartSolid = false;
float _oldLambda = _lambda;
_lambda = _lambda - vw / vr;
if (_lambda > _oldLambda)
{
if (_lambda > 1)
{
return(false);
}
Vector3 bPreCenter = x;
x = r * _lambda;
Vector3 offSet = x - bPreCenter;
Q[0] -= offSet;
Q[1] -= offSet;
Q[2] -= offSet;
supportB += offSet;
support = supportA - supportB;
minDist = maxDist;
nor = closest;
//size=0;
}
}
}
//ASSERT(size < 4); lxq test
B[size] = bIndex;
Q[size++] = support;
//calculate the closest point between two convex hull
closest = GJKCPairDoSimplex(support, ref size);
sDist = Vector3.Dot(closest, closest);
bCon = minDist > sDist;
bNotTerminated = (sDist > inflation2) && bCon;
}
//bool aQuadratic = a.isMarginEqRadius();
//ML:if the Minkowski sum of two objects are too close to the original(eps2 > sDist), we can't take v because we will lose lots of precision. Therefore, we will take
//previous configuration's normal which should give us a reasonable approximation. This effectively means that, when we do a sweep with inflation, we always keep v because
//the shapes converge separated. If we do a sweep without inflation, we will usually use the previous configuration's normal.
nor = ((sDist > eps2) && bCon) ? closest : nor;
nor = Vector3.Normalize(nor);
normal = nor;
lambda = _lambda;
//float cosAngle = Vector3.Dot(nor, r);
//float offset = 0.001f / cosAngle;
//lambda -= offset;
if (lambda < 0)
{
//lambda *= cosAngle;
lambda = 0;
}
points.size = size;
points.a = B[0];
points.b = B[1];
points.c = B[2];
Vector3 closestP = bCon? closest: prevClosest;
closeB = getClosestPoint(closestP, size, b);
//closestA = V3Sel(aQuadratic, V3NegScaleSub(nor, a.getMargin(), closA), closA);
StartSolid = false;
if (_StartSolid)
{
GJKType ret = GjkLocalPenetration_CapsuleConvex(a, b, ref normal, ref lambda, ref points, ref closeB);
if (ret == GJKType.EPA_CONTACT)
{
StartSolid = true;
}
else
{
if (lambda > 0)
{
lambda = 0;
}
}
}
return(true);
}
