/// <summary>
/// Performs a local motion planning to estimate the next pose
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a resulting transform
MTransform result = new MTransform();
//Estimate the delta
MVector3 deltaPosition = targetPosition.Subtract(currentPosition);
//Estimate the meximum allowed delta
double maxTranslationDelta = velocity * time.TotalSeconds;
//Limit the maximum
if (deltaPosition.Magnitude() >= maxTranslationDelta)
{
deltaPosition = deltaPosition.Normalize();
deltaPosition = deltaPosition.Multiply(maxTranslationDelta);
}
float angularVelocityReach = 100f;
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxAngle = angularVelocityReach * time.TotalSeconds;
//Estimate the blendweihgt for the oreitnation blending
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(deltaPosition);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs the actual motion planning
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(float velocity, float angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
MTransform result = new MTransform();
MVector3 delta = targetPosition.Subtract(currentPosition);
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxTranslationDelta = velocity * time.TotalSeconds;
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//To do consider self collision
double maxAngle = angularVelocity * time.TotalSeconds;
if (angle < maxAngle)
{
angle = maxAngle;
}
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(delta);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Computes the rotation to rotate from one vector to the other
/// </summary>
/// <param name="from">The start direction</param>
/// <param name="to">The desired direction</param>
/// <returns></returns>
private static MQuaternion FromToRotation(MVector3 from, MVector3 to)
{
//Normalize both vectors
from = from.Normalize();
to = to.Normalize();
//Estimate the rotation axis
MVector3 axis = MVector3Extensions.Cross(from, to).Normalize();
//Compute the phi angle
double phi = Math.Acos(MVector3Extensions.Dot(from, to)) / (from.Magnitude() * to.Magnitude());
//Create a new quaternion representing the rotation
MQuaternion result = new MQuaternion()
{
X = Math.Sin(phi / 2) * axis.X,
Y = Math.Sin(phi / 2) * axis.Y,
Z = Math.Sin(phi / 2) * axis.Z,
W = Math.Cos(phi / 2)
};
//Perform is nan check and return identity quaternion
if (double.IsNaN(result.W) || double.IsNaN(result.X) || double.IsNaN(result.Y) || double.IsNaN(result.Z))
{
result = new MQuaternion(0, 0, 0, 1);
}
//Return the estimated rotation
return(result);
}
/// <summary>
/// Returns the forward vector of the root transform
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MVector3 GetRootForwad(MAvatarPostureValues posture)
{
MTransform rootTransform = this.GetRootTransform(posture);
//Compute the forwad vector of the root transform
MVector3 rootForward = rootTransform.Rotation.Multiply(new MVector3(0, 0, 1));
rootForward.Y = 0;
rootForward = rootForward.Normalize();
return(rootForward);
}
/// <summary>
/// Returns the forward vector of the root transform
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MVector3 GetGlobalDirection(MAvatarPostureValues posture, MVector3 localDireciton)
{
MTransform rootTransform = this.GetRootTransform(posture);
//Compute the forwad vector of the root transform
MVector3 rootForward = rootTransform.Rotation.Multiply(localDireciton);
rootForward.Y = 0;
rootForward = rootForward.Normalize();
return(rootForward);
}
/// <summary>
/// Implementation based on https://gamedevelopment.tutsplus.com/tutorials/understanding-steering-behaviors-collision-avoidance--gamedev-7777
/// </summary>
/// <param name="position"></param>
/// <param name="velocity"></param>
private MVector3 ComputCollisionAvoidance(MVector3 position, MVector3 velocity)
{
MVector3 normalizedVelocity = velocity.Normalize();
float MAX_SEE_AHEAD = 0.4f;
float MAX_AVOID_FORCE = 5f;
//ahead = position + normalize(velocity) * MAX_SEE_AHEAD
MVector3 ahead = position.Add(normalizedVelocity.Multiply(MAX_SEE_AHEAD));
MVector3 ahead2 = position.Add(normalizedVelocity.Multiply(MAX_SEE_AHEAD * 0.5f));
///The obstacles describing the body
List <Obstacle> obstacles = new List <Obstacle>();
MVector3 pelvisPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
MVector3 headPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.HeadJoint);
obstacles.Add(new Obstacle()
{
Center = pelvisPosition,
Radius = 0.45f
});
obstacles.Add(new Obstacle()
{
Center = headPosition,
Radius = 0.3f
});
Obstacle mostThreatening = findMostThreateningObstacle(position, ahead, ahead2, obstacles);
MVector3 avoidance = new MVector3(0, 0, 0);
if (mostThreatening != null)
{
avoidance.X = ahead.X - mostThreatening.Center.X;
avoidance.Y = ahead.Y - mostThreatening.Center.Y;
avoidance.Z = ahead.Z - mostThreatening.Center.Z;
avoidance = avoidance.Normalize();
avoidance = avoidance.Multiply(MAX_AVOID_FORCE);
}
else
{
avoidance = avoidance.Multiply(0);
}
return(avoidance);
}
/// <summary>
/// Performs local motion planning to reach the defined point
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the vector to reach the goal
MVector3 delta = targetPosition.Subtract(currentPosition);
float distance = delta.Magnitude();
//Determine the angular distance
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Compute the translation weight
float translationWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Compute the rotation weight
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
//Limit the translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//Compute the new position
result.Position = currentPosition.Add(delta);
if (angularVelocity == 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, translationWeight);
}
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
return(result);
}
/// <summary>
/// Do step routine in which the actual simulation result is generated
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = new List <MSceneManipulation>()
};
this.SkeletonAccess.SetChannelData(simulationState.Current.Copy());
// Target position we want to transform to
MVector3 targetPos = this.targetTransform.Position;
// Fully body posture. Only Pelvis Center (first joint) has to be manipulated.
List <double> posture = simulationState.Current.PostureData;
// Current position and distance to target.
MVector3 currentPos = this.SkeletonAccess.GetRootPosition(simulationState.Initial.AvatarID);
MVector3 distance = targetPos.Subtract(currentPos);
// Current rotation and rotational diff to target
MQuaternion currentRot = this.SkeletonAccess.GetRootRotation(simulationState.Initial.AvatarID);
MQuaternion targetRot = this.targetTransform.Rotation;
MVector3 newPos;
MVector3 deltaDistance = distance.Clone();
if (this.velocity > 0)
{
deltaDistance = distance.Normalize().Multiply(this.velocity * time);
Console.WriteLine("Delta v: " + deltaDistance.Magnitude() + " " + time + " " + this.velocity);
}
// If no velocity set or distance very close, directly morph to target position and rotation.
if (this.velocity <= 0 || distance.Magnitude() < deltaDistance.Magnitude())
{
newPos = targetPos;
// Set rotation
//posture[3] = this.targetTransform.Rotation.W;
//posture[4] = this.targetTransform.Rotation.X;
//posture[5] = this.targetTransform.Rotation.Y;
//posture[6] = this.targetTransform.Rotation.Z;
// Add end event.
Console.WriteLine("Finished with vel " + this.velocity + " at " + distance.Magnitude());
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
else // if velocity > 0 and distance sufficiently large, we should apply linear translation with the provided velocity.
{
newPos = currentPos.Add(deltaDistance);
Console.WriteLine("Target Location: " + this.targetTransform.Position + " " + currentPos + " " + distance + " " + deltaDistance + " " + newPos);
}
Console.WriteLine("newposrot: " + newPos + " " + targetRot);
this.SkeletonAccess.SetRootPosition(simulationState.Current.AvatarID, newPos);
this.SkeletonAccess.SetRootRotation(simulationState.Current.AvatarID, targetRot);
result.Posture = this.SkeletonAccess.GetCurrentPostureValues(simulationState.Current.AvatarID);
Console.WriteLine("Frame : " + result.Posture.PostureData[0] + " " + result.Posture.PostureData[1] + " " + result.Posture.PostureData[2] + " " + result.Posture.PostureData[3] + " " + result.Posture.PostureData[4] + " " + result.Posture.PostureData[5] + " " + result.Posture.PostureData[6] + " ");
//Return the result
return(result);
}
/// <summary>
/// Method performs a local motion planning and tries to reach the specified goal position and rotation using the given velocity,angular velocity and time.
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation, bool collisionAvoidance)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the delta
MVector3 delta = targetPosition.Subtract(currentPosition);
//Determine the current delta angle
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle in the current frame
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Estimate the blend weight for the rotation and position
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
float positionWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
if (collisionAvoidance)
{
MVector3 collisionAvoidanceForce = this.ComputCollisionAvoidance(currentPosition, delta);
//if (collisionAvoidanceForce.Magnitude() > 0)
// MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Collision avoidance force: " + collisionAvoidanceForce.Magnitude());
//Add the collision avoidance force on top
delta = delta.Add(collisionAvoidanceForce);
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
}
//Compute the new position
result.Position = currentPosition.Add(delta);
//Compute the new rotation by interpolating towards the target rotation
if (angularVelocity > 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
//Use the rotation weight
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, positionWeight);
}
//Return the simulation result
return(result);
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>()
};
//Assign the constraints to a temp varilable
List <MConstraint> constraints = result.Constraints;
//Use the constraint manager to manage the constraints
constraintManager.SetConstraints(ref constraints);
//Get the hand position and rotation of the last frame (approved result)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Get the desired hand position (of the underlying motion e.g. idle)
this.SkeletonAccess.SetChannelData(simulationState.Current);
MVector3 targetHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion targetHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Add an offset on top of the position if desired
if (this.addOffset)
{
targetHandPosition = ComputeNewPositionWithOffset(targetHandPosition, simulationState.Current);
}
//Move the hand from the current position to the target position
MVector3 deltaPosition = targetHandPosition.Subtract(currentHandPosition);
//Compute the distance of the hand to the target hand position
float distanceToGoal = deltaPosition.Magnitude();
//Create positioning finished event if not already created and distance below threshold
if (distanceToGoal < this.positioningFinishedThreshold && !this.positioningFinished)
{
result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", this.instruction.ID));
this.positioningFinished = true;
}
//Compute the current velocity based on the general max velocity and the velocity of the root motion
float currentVelocity = this.velocity + this.ComputeRootVelocity(time, simulationState);
//Compute the max distance which can be covered within the current frame
float maxDistance = (float)(time * currentVelocity);
//Compute the weight for slerping (weight increases with shrinking distance to target)
float weight = Math.Max(0, 1 - distanceToGoal);
//Create a new transform representing the next hand transform
MTransform newHandTransform = new MTransform("", currentHandPosition.Clone(), currentHandRotation.Clone())
{
//Compute the new hand position (normalize delta position and multiply by max distance)
Position = currentHandPosition.Add(deltaPosition.Normalize().Multiply(Math.Min(deltaPosition.Magnitude(), maxDistance))),
//Just perform an interpolation to gather new hand rotation (weight is determined by the translation distance)
Rotation = MQuaternionExtensions.Slerp(currentHandRotation, targetHandRotation, weight)
};
//Compute the corresponding positon/rotation of the object and
//adjust the transformation of the object which should be moved
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = this.objectTransform.ID,
//Compute the new global position of the object
Position = newHandTransform.TransformPoint(this.objectPositionOffset),
//Compute the new global rotation of the object
Rotation = newHandTransform.TransformRotation(this.objectRotationOffset)
}
}
});
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, newHandTransform.Position, newHandTransform.Rotation);
//Generate a new posture using the ik solver and the specified constraints
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
//Return the result
return(result);
}
/// <summary>
/// Default do step routine
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Set the channel data to reflect to current posture
SkeletonAccess.SetChannelData(simulationState.Current);
//Set the default rotation of the head and neck joints
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint, initialHeadRotation);
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint, initialNeckRotation);
//Create a transform representing the current head location
MTransform currentTransform = new MTransform()
{
ID = "",
Position = SkeletonAccess.GetGlobalJointPosition(AvatarDescription.AvatarID, MJointType.HeadJoint),
Rotation = SkeletonAccess.GetGlobalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint)
};
//Create a transform representing the parent location (neck)
MTransform parentTransform = new MTransform()
{
ID = "",
Position = SkeletonAccess.GetGlobalJointPosition(AvatarDescription.AvatarID, MJointType.C4C5Joint),
Rotation = SkeletonAccess.GetGlobalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint)
};
//The current head forward vector
MVector3 currentHeadForward = new MVector3(-1, 0, 0);
//Compute the local position of the desired object (relative to the neck)
MVector3 localPosition = parentTransform.InverseTransformPoint(this.gazeTarget.Position);
//Get the xz distance in local space
float distance = new MVector3(localPosition.X, 0, localPosition.Z).Magnitude();
float height = (float)localPosition.Y;
//Compute the current angle
float currentAngle = (float)(Math.Atan(height / distance) * 180 / Math.PI);
//Limit if below lower limit
if (currentAngle < lowerLimit)
{
localPosition.Y = Math.Tan(lowerLimit * Math.PI / 180) * distance;
}
//Limit if above upper angle limit
if (currentAngle > upperLimit)
{
localPosition.Y = Math.Tan(upperLimit * Math.PI / 180) * distance;
}
float maxYAngle = 80f;
//Limit xz position
float yAngle = (float)MVector3Extensions.Angle(currentHeadForward, new MVector3(localPosition.X, 0, localPosition.Z));
if (yAngle > maxYAngle)
{
//The interpolated direction
MVector3 interpolatedDirection = MVector3Extensions.Lerp(currentHeadForward, new MVector3(localPosition.X, 0, localPosition.Z).Normalize(), (maxYAngle / yAngle)).Normalize();
//Perform correction
MVector3 newLocalPositionsXZ = interpolatedDirection.Multiply(distance);
localPosition.X = newLocalPositionsXZ.X;
localPosition.Z = newLocalPositionsXZ.Z;
}
//Compute the desired and current facing direction
MVector3 desiredHeadForward = localPosition.Normalize();
//Estimate the rotation that is required to rotate from the current head direction towards the desired one
MQuaternion deltaRotation = FromToRotation(currentHeadForward, new MVector3(desiredHeadForward.X, -desiredHeadForward.Y, desiredHeadForward.Z));
//Gather the current location rotation
MQuaternion currentLocalRotation = SkeletonAccess.GetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint);
//Update the local joint rotation to adjust the facing direction to the desired values
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint, currentLocalRotation.Multiply(deltaRotation));
//Set the updated postures
result.Posture = SkeletonAccess.RecomputeCurrentPostureValues(AvatarDescription.AvatarID);
//Return the simulation results
return(result);
}
/// <summary>
/// Do step routine in which the actual simulation result is generated
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events != null ? simulationState.Events : new List <MSimulationEvent>(),
Constraints = simulationState.Constraints,
SceneManipulations = simulationState.SceneManipulations != null ? simulationState.SceneManipulations : new List <MSceneManipulation>()
};
//Create variables representing the next object position/rotation
MTransform nextObjectTransform = subjectTransform.Clone();
//Use the constraint manager to manage the constraints
List <MConstraint> tmpConstraints = result.Constraints;
//Set the constraints
constraintManager.SetConstraints(ref tmpConstraints);
//Compute the new hand position and rotation
MVector3 deltaPosition = this.targetObjectTransform.Position.Subtract(subjectTransform.Position);
float distanceToGoal = deltaPosition.Magnitude();
//Get the current object position
float maxDistance = (float)time * 1.0f;
//Check the current distance to goal
if (distanceToGoal < 0.01f)
{
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
else
{
//Compute the new hand position (normalize delta position and multiply by max distance)
nextObjectTransform.Position = this.subjectTransform.Position.Add(deltaPosition.Normalize().Multiply(Math.Min(distanceToGoal, maxDistance)));
//Compute the weight for slerping (weight increases with shrinking distance to target)
float weight = Math.Max(0, 1 - distanceToGoal);
//Just perform an interpolation to gather new hand rotation (weight is determined by the translation distance)
nextObjectTransform.Rotation = MQuaternionExtensions.Slerp(this.subjectTransform.Rotation, this.targetObjectTransform.Rotation, weight);
}
//Adjust the transformation of the object which should be moved
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = this.subjectTransform.ID,
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
//Get the current hand position in global space
MVector3 globalHandPosition = nextObjectTransform.TransformPoint(this.handPositionOffset);
MQuaternion globalHandRotation = nextObjectTransform.TransformRotation(this.handRotationOffset);
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, globalHandPosition, globalHandRotation);
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
//Generate a new posture using the ik solver and the specified constraints
result.Posture = ikResult.Posture;
//Return the result
return(result);
}
public MAvatarPosture RetargetToTarget(MAvatarPostureValues intermediatePostureValues)
{
string id = intermediatePostureValues.AvatarID;
RJoint root = ((RJoint)this.skeleton.GetRoot(id));
root.SetAvatarPostureValues(intermediatePostureValues);
MAvatarPosture targetOut = new MAvatarPosture();
targetOut.AvatarID = id;
targetOut.Joints = new List <MJoint>();
foreach (MJoint j in this.basePostures[id].Joints)
{
MJoint outJ = new MJoint();
outJ.ID = j.ID;
outJ.Type = j.Type;
outJ.Parent = j.Parent;
if (outJ.Type != MJointType.Undefined)
{
RJoint rj = (RJoint)root.GetChild(j.Type);
outJ.Position = (rj).RetargetPositionToTarget();
outJ.Rotation = (rj).RetargetRotationToTarget();
}
else
{
outJ.Position = j.Position;
outJ.Rotation = j.Rotation;
}
targetOut.Joints.Add(outJ);
}
Dictionary <string, string> _children = this.children[id];
for (int i = 0; i < targetOut.Joints.Count; i++)
{
MJoint outJ = targetOut.Joints[i];
if (outJ.Type == MJointType.Undefined)
{
bool setRot = false;
bool setPos = false;
Console.WriteLine("no jointtype " + outJ.ID);
if (i == 0)
{
// find first joint that is mapped
foreach (MJoint j in targetOut.Joints)
{
if (j.Type != MJointType.Undefined)
{
outJ.Position = new MVector3(j.Position.X, 0, j.Position.Z);
//j.Position.X = 0;
//j.Position.Z = 0;
MVector3 forward = outJ.Rotation.Multiply(new MVector3(0, 0, 1));
forward.Y = 0;
forward.Normalize();
MVector3 currentForward = j.Rotation.Multiply(new MVector3(0, 0, 1));
MQuaternion drot = MVector3Extensions.FromToRotation(currentForward, forward);
outJ.Rotation = drot.Multiply(j.Rotation);
//outJ.Rotation = MQuaternionExtensions.Inverse(drot).Multiply(outJ.Rotation);
setPos = true;
setRot = true;
break;
}
}
}
else
{
/*
* This is disabled for now, as it was not working propperly.
*
* if(_children.ContainsKey(outJ.ID) && _children[outJ.ID] != "")
* {
* for(int jID = i+1; jID < targetOut.Joints.Count; jID ++)
* {
* MJoint j = targetOut.Joints[jID];
* if (j.ID == _children[outJ.ID])
* {
*
* MVector3 srcDir = new MVector3(0, 1, 0);//outJ.Rotation.Multiply(new MVector3(0, 1, 0)).Normalize
* MVector3 trgDir = null;
* MQuaternion parentRot = null;
* if(outJ.Parent != null)
* {
* for(int pID = i-1; pID > 0; pID--)
* {
* if(targetOut.Joints[pID].ID == outJ.Parent)
* {
* if(targetOut.Joints[pID].Type != MJointType.Undefined)
* {
* parentRot = targetOut.Joints[pID].Rotation;
* trgDir = MQuaternionExtensions.Inverse(parentRot).Multiply(j.Position.Subtract(outJ.Position).Normalize());
* }
* }
* }
* }
* if(trgDir != null)
* {
* MQuaternion rot = MVector3Extensions.FromToRotation(srcDir, trgDir);
* outJ.Rotation = parentRot.Multiply(rot);
* outJ.Position = null;
* setRot = true;
* break;
*
* }
*
*
* }
* }
* }*/
}
if (!setRot)
{
outJ.Rotation = null;
}
if (!setPos)
{
outJ.Position = null;
}
}
}
return(targetOut);
}