/// <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); }