public override void OnDrawRuntimeGizmos(RuntimeGizmos.RuntimeGizmoDrawer drawer)
{
base.OnDrawRuntimeGizmos(drawer);
// Grasp Point
float graspAmount = 0F;
if (graspAxisOverride != null)
{
graspAmount = graspAxisOverride();
}
else
{
try {
graspAmount = Input.GetAxis(graspButtonAxis);
}
catch (ArgumentException) { }
}
drawer.color = Color.Lerp(GizmoColors.GraspPoint, Color.white, graspAmount);
drawer.DrawWireSphere(GetGraspPoint(), maxGraspDistance);
// Nearest graspable object
if (_closestGraspableObject != null)
{
drawer.color = Color.Lerp(GizmoColors.Graspable, Color.white, Mathf.Sin(Time.time * 2 * Mathf.PI * 2F));
drawer.DrawWireSphere(_closestGraspableObject.rigidbody.position, maxGraspDistance * 0.75F);
}
}
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 DrawPoseSpline(this RuntimeGizmos.RuntimeGizmoDrawer drawer,
HermitePoseSpline spline,
Color?color = null,
float poseGizmoScale = 0.02f,
int splineResolution = 32,
int drawPosePeriod = 8,
bool drawPoses = true,
bool drawSegments = true)
{
if (!color.HasValue)
{
color = LeapColor.brown.WithAlpha(0.4f);
}
drawer.color = color.Value;
var tWidth = spline.maxT - spline.minT;
Vector3?prevPos = null;
int counter = 0;
float tStep = (1f / splineResolution) * tWidth;
for (float t = spline.minT; t <= spline.minT + tWidth; t += tStep)
{
var pose = spline.PoseAt(t);
if (counter % drawPosePeriod == 0 && drawPoses)
{
drawer.DrawPose(pose, 0.02f);
}
if (prevPos.HasValue && drawSegments)
{
drawer.DrawLine(prevPos.Value, pose.position);
}
prevPos = pose.position;
counter++;
}
}
public static void DrawRay(this RuntimeGizmos.RuntimeGizmoDrawer drawer,
Vector3 position, Vector3 direction)
{
drawer.DrawLine(position, position + direction);
}
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);
}
}
