/// <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); } }