public void TestPoses()
{
Random.InitState(0);
var p = new Pose(Random.insideUnitSphere, Random.rotation);
var v = Random.insideUnitSphere;
var w = PoseUtils.WorldFromLocalPoint(p, v);
var v2 = PoseUtils.LocalFromWorldPoint(p, w);
AssertNear(v, v2, 0.001f);
}
public static Pose CalcGoalPose(Pose myPose, Pose targetPose, ProxyAlignmentForceType forceType, Pose relativePose)
{
var goalPose = targetPose;
switch (forceType)
{
case ProxyAlignmentForceType.MoveToAndAlignWith:
break; // simply the other pose
case ProxyAlignmentForceType.MoveToAndFace:
goalPose.rotation = PoseUtils.LookRotationSafe(targetPose.position - myPose.position, Vector3.up);
break; // done
case ProxyAlignmentForceType.MoveToAndFaceAwayFrom:
goalPose.rotation = PoseUtils.LookRotationSafe(targetPose.position - myPose.position, Vector3.up);
goalPose.rotation = goalPose.rotation * Quaternion.Euler(0, 180f, 0);
break; // done
case ProxyAlignmentForceType.CenterInFrontOfAndFace:
{
var ray = new Ray(targetPose.position, targetPose.forward);
var dist = Vector3.Dot(myPose.position - ray.origin, ray.direction.normalized);
var nearest = ray.GetPoint(dist);
goalPose.position = nearest;
goalPose.rotation = PoseUtils.LookRotationSafe(targetPose.position - goalPose.position, Vector3.up);
}
break;
case ProxyAlignmentForceType.SceneInitialRelativePose:
goalPose = PoseUtils.WorldFromLocalPose(targetPose, relativePose);
break;
case ProxyAlignmentForceType.SceneInitialRelativeAngle:
{
var relGoal = PoseUtils.WorldFromLocalPose(targetPose, relativePose);
var ray = new Ray(relGoal.position, (relGoal.position - targetPose.position).normalized);
var dist = Vector3.Dot(myPose.position - ray.origin, ray.direction.normalized);
var nearest = ray.GetPoint(dist);
goalPose = new Pose(nearest, relGoal.rotation);
}
break;
}
return(goalPose);
}
public Vector3 LocalFromWorldPoint(Vector3 pnt)
{
return(PoseUtils.LocalFromWorldPoint(Pose, pnt));
}
public Vector3 WorldFromLocalVector(Vector3 pnt)
{
return(PoseUtils.WorldFromLocalVector(Pose, pnt));
}
public Vector3 LocalFromUnitPoint(Vector3 pnt)
{
return(PoseUtils.ScaleVector3(pnt, vectorRadii));
}
internal DistanceSample SampleDistanceVectorFromPointLocal(Vector3 localPos, ShapeSample shape)
{
if (verticesInXZ != null)
{
var polygon = verticesInXZ;
var nearestPoint = localPos;
if ((shape == ShapeSample.Fill) && GeometryUtils.PointInPolygon(localPos, polygon))
{
nearestPoint.y = 0.0f;
}
else
{
nearestPoint = ClosetPointOnPolygonEdge(localPos, polygon);
}
var toSurf = (nearestPoint - localPos);
return(new DistanceSample(toSurf.magnitude, toSurf));
}
if (primitiveType == FieldPrimitiveType.Box)
{
// Note: C0 not C1 continuous
if (shape == ShapeSample.Fill)
{
var nearest = PoseUtils.ScaleVector3(PoseUtils.Min(PoseUtils.Abs(localPos), vectorRadii), PoseUtils.Sign(localPos));
var b = PoseUtils.Abs(localPos) - (vectorRadii);
var dist = Mathf.Max(b.x, Mathf.Max(b.y, b.z));
if (dist < 0.0f)
{
b = vectorRadii - PoseUtils.Abs(localPos);
nearest = localPos;
var nearestTip = PoseUtils.ScaleVector3(vectorRadii, PoseUtils.Sign(localPos));
if (PoseUtils.IsXTheMin(b))
{
nearest.x = nearestTip.x;
}
else if (PoseUtils.IsYTheMin(b))
{
nearest.y = nearestTip.y;
}
else
{
nearest.z = nearestTip.z;
}
}
else
{
dist = (nearest - localPos).magnitude;
}
var toSurf = (nearest - localPos);
return(new DistanceSample(dist, toSurf));
}
else if (shape == ShapeSample.Edge)
{
var absLocal = PoseUtils.Abs(localPos);
var absUnit = PoseUtils.Abs(PoseUtils.InvScaleVector3(localPos, vectorRadii) - Vector3.one);
var absNearest = vectorRadii;
if (PoseUtils.IsXTheMin(absUnit))
{
absNearest = new Vector3(absLocal.x, vectorRadii.y, vectorRadii.z);
}
else if (PoseUtils.IsYTheMin(absUnit))
{
absNearest = new Vector3(vectorRadii.x, absLocal.y, vectorRadii.z);
}
else
{
absNearest = new Vector3(vectorRadii.x, vectorRadii.y, absLocal.z);
}
var nearest = PoseUtils.ScaleVector3(absNearest, PoseUtils.Sign(localPos));
var toSurf = (nearest - localPos);
var dist = toSurf.magnitude;
return(new DistanceSample(dist, toSurf));
}
else
{
throw new System.NotImplementedException("TODO:" + shape);
}
}
else if (primitiveType == FieldPrimitiveType.Sphere)
{
var nearest = PoseUtils.ScaleVector3(localPos.normalized, vectorRadii);
var dist = (localPos.magnitude - nearest.magnitude);
return(new DistanceSample(dist, nearest - localPos));
}
else if (primitiveType == FieldPrimitiveType.PlaneForward)
{
if (shape == ShapeSample.Edge)
{
throw new System.NotImplementedException("TODO: Plane+Edge");
}
Vector3 nearest;
var flipDistance = false;
if ((shape == ShapeSample.Fill) && (localPos.z >= 0))
{
nearest = new Vector3(
Mathf.Clamp(localPos.x, -vectorRadii.x, vectorRadii.x),
Mathf.Clamp(localPos.y, -vectorRadii.y, vectorRadii.y),
Mathf.Clamp(localPos.z, -vectorRadii.z, vectorRadii.z)
);
nearest.z = 0.0f;
}
else // if edge or if behind the plane
{
nearest = PoseUtils.ScaleVector3(vectorRadii, PoseUtils.Sign(localPos));
nearest.z = 0.0f;
}
var dist = (localPos.magnitude - nearest.magnitude) * (flipDistance ? -1.0f : 1.0f);
return(new DistanceSample(dist, nearest - localPos));
}
else if (primitiveType == FieldPrimitiveType.LineOnX)
{
var radius = vectorRadii.x;
var fx = Mathf.Clamp(localPos.x, -radius, radius);
var nearest = new Vector3(fx, 0, 0);
var dist = nearest.magnitude;
return(new DistanceSample(dist, nearest));
}
else
{
throw new System.NotImplementedException("TODO.");
}
}