/// <summary>
/// If collision happened project the ParticleObject back to prevCenterMass.
/// </summary>
/// <param name="b1"></param>
/// <param name="b2"></param>
public void CollisionResolutionOBB(CollisionManifold cm, ColliderBox b1, ColliderBox b2)
{
Vector3 currPoint = cm.avg_depth;
Vector3 projPoint = cm.avg_contact;
float dist = Vector3.Magnitude(currPoint - projPoint);
if (Util.CMP(dist, 0.0f) || float.IsNaN(dist))
{
return;
}
if (!b1.IsStatic() && !b2.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint, b2.GetParticleObject());
}
else
{
if (!b1.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint);
}
else if (!b2.IsStatic())
{
this.SeparateParticleObjects(b2.GetParticleObject(), projPoint, currPoint);
}
}
}
public static void CreateCapsulesFromSegments([NotNull] NativeList <float4x2> lineBuffer, float scale,
[NotNull] Mesh mesh)
{
if (lineBuffer == null)
{
throw new ArgumentNullException(nameof(lineBuffer));
}
if (mesh == null)
{
throw new ArgumentNullException(nameof(mesh));
}
if (lineBuffer.Length == 0)
{
mesh.Clear();
return;
}
int length = 10 * lineBuffer.Length;
using (var points = new Rent <Vector3>(length))
using (var colors = new Rent <Color32>(length))
using (var uvs = new Rent <Vector2>(length))
using (var indices = new Rent <int>(CapsuleIndices.Length * lineBuffer.Length))
{
int baseOff = 0;
int pOff = 0;
int cOff = 0;
int uvOff = 0;
int iOff = 0;
const float minMagnitude = 1e-8f;
foreach (ref readonly float4x2 line in lineBuffer.Ref())
{
Vector3 a = line.c0.xyz;
Vector3 b = line.c1.xyz;
Vector3 dirX = b - a;
Vector3 dirY, dirZ;
float dirXMagnitude = dirX.Magnitude();
if (dirXMagnitude < minMagnitude)
{
dirX = Vector3.zero;
dirY = Vector3.zero;
dirZ = Vector3.zero;
}
else
{
dirX /= dirX.Magnitude();
dirY = new Vector3(-dirX.y, dirX.x, 0);
if (Mathf.Abs(dirY.MaxAbsCoeff()) < minMagnitude)
{
dirY = Vector3.up.Cross(dirX);
}
dirX *= scale;
dirY *= scale / dirY.Magnitude();
dirZ = dirX.Cross(dirY);
dirZ *= scale / dirZ.Magnitude();
}
Vector3 halfDirX = 0.5f * dirX;
Vector3 halfSumYz = 0.5f * (dirY + dirZ);
Vector3 halfDiffYz = 0.5f * (dirY - dirZ);
points.Array[pOff++] = a - halfDirX;
points.Array[pOff++] = a + halfSumYz;
points.Array[pOff++] = a + halfDiffYz;
points.Array[pOff++] = a - halfSumYz;
points.Array[pOff++] = a - halfDiffYz;
points.Array[pOff++] = b + halfSumYz;
points.Array[pOff++] = b + halfDiffYz;
points.Array[pOff++] = b - halfSumYz;
points.Array[pOff++] = b - halfDiffYz;
points.Array[pOff++] = b + halfDirX;
Color32 ca = PointWithColor.ColorFromFloatBits(line.c0.w);
Color32 cb = PointWithColor.ColorFromFloatBits(line.c1.w);
Vector2 uv0 = new Vector2(line.c0.w, 0);
for (int i = 0; i < 5; i++)
{
colors.Array[cOff++] = ca;
uvs.Array[uvOff++] = uv0;
}
Vector2 uv1 = new Vector2(line.c1.w, 0);
for (int i = 5; i < 10; i++)
{
colors.Array[cOff++] = cb;
uvs.Array[uvOff++] = uv1;
}
foreach (int index in CapsuleIndices)
{
indices.Array[iOff++] = baseOff + index;
}
baseOff += 10;
}
mesh.Clear();
mesh.SetVertices(points);
mesh.SetTriangles(indices);
mesh.SetColors(colors);
mesh.SetUVs(uvs);
}
}