public static void DrawPose(this RuntimeGizmos.RuntimeGizmoDrawer drawer,
Pose pose, float radius = 0.10f,
bool drawCube = false)
{
drawer.PushMatrix();
drawer.matrix = Matrix4x4.TRS(pose.position, pose.rotation, Vector3.one);
var origColor = drawer.color;
//drawer.DrawWireSphere(Vector3.zero, radius);
if (drawCube)
{
drawer.DrawCube(Vector3.zero, Vector3.one * radius * 0.3f);
}
drawer.DrawPosition(Vector3.zero, radius * 2);
drawer.color = origColor;
drawer.PopMatrix();
}
public static void FillPolyMesh(this IPositionSpline spline,
PolyMesh polyMesh,
Matrix4x4?applyTransform = null,
float?startT = null, float?endT = null,
int?numSegments = null,
float[] radii = null,
float?radius = null,
bool drawDebug = false)
{
float minT, maxT;
int effNumSegments;
//bool useRadiusArr = false;
//float[] effRadii;
float effRadius;
bool useTransform;
Matrix4x4 transform;
RuntimeGizmos.RuntimeGizmoDrawer drawer = null;
if (drawDebug)
{
RuntimeGizmos.RuntimeGizmoManager.TryGetGizmoDrawer(out drawer);
}
// Assign parameters based on optional inputs
{
minT = spline.minT;
if (startT.HasValue)
{
minT = startT.Value;
}
maxT = spline.maxT;
if (endT.HasValue)
{
maxT = endT.Value;
}
effNumSegments = 32;
if (numSegments.HasValue)
{
effNumSegments = numSegments.Value;
effNumSegments = Mathf.Max(1, effNumSegments);
}
//useRadiusArr = false;
effRadius = 0.02f;
if (radius.HasValue)
{
effRadius = radius.Value;
}
//effRadii = null;
//if (radii != null) {
// useRadiusArr = true;
// effRadii = radii;
//}
useTransform = false;
transform = Matrix4x4.identity;
if (applyTransform.HasValue)
{
useTransform = true;
transform = applyTransform.Value;
}
}
// Multiple passes through the spline data will construct all the positions and
// orientations we need to build the mesh.
polyMesh.Clear();
var crossSection = new CircularCrossSection(effRadius, 16);
float tStep = (maxT - minT) / effNumSegments;
Vector3 position = Vector3.zero;
Vector3 dPosition = Vector3.zero;
Vector3?tangent = null;
var positions = Pool <List <Vector3> > .Spawn();
positions.Clear();
var normals = Pool <List <Vector3> > .Spawn(); // to start, normals contain velocities,
normals.Clear(); // but zero velocities are filtered out.
var binormals = Pool <List <Vector3> > .Spawn();
binormals.Clear();
var crossSection0Positions = Pool <List <Vector3> > .Spawn();
crossSection0Positions.Clear();
var crossSection1Positions = Pool <List <Vector3> > .Spawn();
crossSection1Positions.Clear();
try {
// Construct a rough list of positions and normals for each cross section. Some
// of the normals may be zero, so we'll have to fix those.
for (int i = 0; i <= effNumSegments; i++)
{
var t = minT + i * tStep;
spline.ValueAndDerivativeAt(t, out position, out dPosition);
if (useTransform)
{
positions.Add(transform.MultiplyPoint3x4(position));
normals.Add(transform.MultiplyVector(dPosition).normalized);
}
else
{
positions.Add(position);
normals.Add(dPosition.normalized);
}
if (!tangent.HasValue && dPosition.sqrMagnitude > 0.001f * 0.001f)
{
tangent = (transform * dPosition.WithW(1)).ToVector3().normalized.Perpendicular();
}
}
// In case we never got a non-zero velocity, try to construct a tangent based on
// delta positions.
if (!tangent.HasValue)
{
if (positions[0] == positions[1])
{
// No spline mesh possible; there's no non-zero length segment.
return;
}
else
{
var delta = positions[1] - positions[0];
// Very specific case: Two points, each with zero velocity, use delta for
// normals
if (positions.Count == 2)
{
normals[0] = delta; normals[1] = delta;
}
tangent = delta.Perpendicular();
}
}
// Try to propagate non-zero normals into any "zero" normals.
for (int i = 0; i <= effNumSegments; i++)
{
if (normals[i].sqrMagnitude < 0.00001f)
{
if (i == 0)
{
normals[i] = normals[i + 1];
}
else if (i == effNumSegments)
{
normals[i] = normals[i - 1];
}
else
{
normals[i] = Vector3.Slerp(normals[i - 1], normals[i + 1], 0.5f);
}
}
if (normals[i].sqrMagnitude < 0.00001f)
{
// OK, we tried, but we still have zero normals. Error and fail.
throw new System.InvalidOperationException(
"Unable to build non-zero normals for this spline during PolyMesh "
+ "construction");
}
}
// With a set of normals and a starting tangent vector, we can construct all the
// binormals we need to have an orientation and position for every cross-section.
Vector3?lastNormal = null;
Vector3?lastBinormal = null;
for (int i = 0; i <= effNumSegments; i++)
{
var normal = normals[i];
Vector3 binormal;
if (!lastBinormal.HasValue)
{
binormal = Vector3.Cross(normal, tangent.Value);
}
else
{
var rotFromLastNormal = Quaternion.FromToRotation(lastNormal.Value, normal);
binormal = rotFromLastNormal * lastBinormal.Value;
}
binormals.Add(binormal);
lastNormal = normal;
lastBinormal = binormal;
}
// With positions, normals, and binormals for every cross section, add positions
// and polygons for each cross section and their connections to the PolyMesh.
int cs0Idx = -1, cs1Idx = -1;
for (int i = 0; i + 1 <= effNumSegments; i++)
{
var pose0 = new Pose(positions[i],
Quaternion.LookRotation(normals[i], binormals[i]));
var pose1 = new Pose(positions[i + 1],
Quaternion.LookRotation(normals[i + 1], binormals[i + 1]));
if (drawDebug)
{
drawer.PushMatrix();
drawer.matrix = transform.inverse;
drawer.color = LeapColor.blue;
drawer.DrawRay(pose0.position, normals[i] * 0.2f);
drawer.color = LeapColor.red;
drawer.DrawRay(pose0.position, binormals[i] * 0.2f);
drawer.PopMatrix();
}
bool addFirstPositions = i == 0;
// Add positions from Cross Section definition to reused buffers.
if (addFirstPositions)
{
crossSection.FillPositions(crossSection0Positions, pose0);
}
crossSection.FillPositions(crossSection1Positions, pose1);
// Add positions from buffers into the PolyMesh.
if (addFirstPositions)
{
cs0Idx = polyMesh.positions.Count;
polyMesh.AddPositions(crossSection0Positions);
}
cs1Idx = polyMesh.positions.Count;
polyMesh.AddPositions(crossSection1Positions);
// Add polygons to connect one cross section in the PolyMesh to the other.
crossSection.AddConnectingPolygons(polyMesh, cs0Idx, cs1Idx);
Utils.Swap(ref crossSection0Positions, ref crossSection1Positions);
cs0Idx = cs1Idx;
}
}
finally {
positions.Clear();
Pool <List <Vector3> > .Recycle(positions);
normals.Clear();
Pool <List <Vector3> > .Recycle(normals);
binormals.Clear();
Pool <List <Vector3> > .Recycle(binormals);
crossSection0Positions.Clear();
Pool <List <Vector3> > .Recycle(crossSection0Positions);
crossSection1Positions.Clear();
Pool <List <Vector3> > .Recycle(crossSection1Positions);
}
}