// debug for static arm collisions from collision listener
public void GetMidLevelArmCollisions()
{
IK_Robot_Arm_Controller arm = getArm();
CollisionListener collisionListener = arm.GetComponentInChildren <CollisionListener>();
if (collisionListener != null)
{
List <Dictionary <string, string> > collisions = new List <Dictionary <string, string> >();
foreach (var sc in collisionListener.StaticCollisions())
{
Dictionary <string, string> element = new Dictionary <string, string>();
if (sc.simObjPhysics != null)
{
element["objectType"] = "simObjPhysics";
element["name"] = sc.simObjPhysics.objectID;
}
else
{
element["objectType"] = "gameObject";
element["name"] = sc.gameObject.name;
}
collisions.Add(element);
}
actionFinished(true, collisions);
}
}
private static void continuousMoveFinish <T>(
PhysicsRemoteFPSAgentController controller,
CollisionListener collisionListener,
Transform moveTransform,
System.Action <Transform, T> setProp,
T target,
T resetProp
)
{
bool actionSuccess = true;
string debugMessage = "";
IK_Robot_Arm_Controller arm = controller.GetComponentInChildren <IK_Robot_Arm_Controller>();
var staticCollisions = collisionListener.StaticCollisions();
if (staticCollisions.Count > 0)
{
var sc = staticCollisions[0];
// decide if we want to return to original property or last known property before collision
setProp(moveTransform, resetProp);
// if we hit a sim object
if (sc.isSimObj)
{
debugMessage = "Collided with static sim object: '" + sc.simObjPhysics.name + "', could not reach target: '" + target + "'.";
}
// if we hit a structural object that isn't a sim object but still has static collision
if (!sc.isSimObj)
{
debugMessage = "Collided with static structure in scene: '" + sc.gameObject.name + "', could not reach target: '" + target + "'.";
}
actionSuccess = false;
}
controller.errorMessage = debugMessage;
controller.actionFinished(actionSuccess, debugMessage);
}
public static IEnumerator updateTransformPropertyFixedUpdate <T>(
PhysicsRemoteFPSAgentController controller,
CollisionListener collisionListener,
Transform moveTransform,
T target,
Func <Transform, T> getProp,
Action <Transform, T> setProp,
Func <Transform, T, T> nextProp,
// We could remove this one, but it is a speedup to not compute direction for position update calls at every addToProp call and just outside while
Func <T, T, T> getDirection,
Func <T, T, float> distanceMetric,
float fixedDeltaTime,
bool returnToStartPropIfFailed,
double epsilon = 1e-3
)
{
T originalProperty = getProp(moveTransform);
var previousProperty = originalProperty;
var arm = controller.GetComponentInChildren <IK_Robot_Arm_Controller>();
var ikSolver = arm.gameObject.GetComponentInChildren <FK_IK_Solver>();
// commenting out the WaitForEndOfFrame here since we shoudn't need
// this as we already wait for a frame to pass when we execute each action
// yield return yieldInstruction;
var currentProperty = getProp(moveTransform);
float currentDistance = distanceMetric(target, currentProperty);
T directionToTarget = getDirection(target, currentProperty);
while (currentDistance > epsilon && collisionListener.StaticCollisions().Count == 0)
{
previousProperty = getProp(moveTransform);
T next = nextProp(moveTransform, directionToTarget);
float nextDistance = distanceMetric(target, next);
// allows for snapping behaviour to target when the target is close
// if nextDistance is too large then it will overshoot, in this case we snap to the target
// this can happen if the speed it set high
if (
nextDistance <= epsilon ||
nextDistance > distanceMetric(target, getProp(moveTransform))
)
{
setProp(moveTransform, target);
}
else
{
setProp(moveTransform, next);
}
// this will be a NOOP for Rotate/Move/Height actions
ikSolver.ManipulateArm();
if (!Physics.autoSimulation)
{
Physics.Simulate(fixedDeltaTime);
}
yield return(new WaitForFixedUpdate());
currentDistance = distanceMetric(target, getProp(moveTransform));
}
T resetProp = previousProperty;
if (returnToStartPropIfFailed)
{
resetProp = originalProperty;
}
continuousMoveFinish(
controller,
collisionListener,
moveTransform,
setProp,
target,
resetProp
);
// we call this one more time in the event that the arm collided and was reset
ikSolver.ManipulateArm();
if (!Physics.autoSimulation)
{
Physics.Simulate(fixedDeltaTime);
}
}
