Exemplo n.º 1
0
        /// <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);
        }
Exemplo n.º 2
0
        /// <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);
        }
Exemplo n.º 3
0
        public MVector3 RetargetPositionToIS(MVector3 globalPos, MQuaternion globalRot)
        {
            MVector3 pos = globalPos.Add(globalRot.Multiply(this.targetOffset));

            this.globalPos = pos;
            return(pos);
        }
Exemplo n.º 4
0
        /// <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);
        }
Exemplo n.º 5
0
        /// <summary>
        /// Computes the new (desired) hand position considering the offset of the collider (to avoid self-collisions)
        /// </summary>
        /// <param name="targetHandPosition"></param>
        /// <param name="currentPosture"></param>
        /// <returns></returns>
        private MVector3 ComputeNewPositionWithOffset(MVector3 targetHandPosition, MAvatarPostureValues currentPosture)
        {
            //Optionally ensure that the object does not intersect the avatar
            MCollider collider = this.SceneAccess.GetColliderById(this.objectTransform.ID);

            //Determine the offset based on the respective collider
            float offset = 0;

            if (collider.SphereColliderProperties != null)
            {
                offset = (float)collider.SphereColliderProperties.Radius;
            }

            if (collider.BoxColliderProperties != null)
            {
                offset = (float)collider.BoxColliderProperties.Size.Magnitude();
            }

            if (collider.CapsuleColliderProperties != null)
            {
                offset = Math.Max((float)collider.CapsuleColliderProperties.Height, (float)collider.CapsuleColliderProperties.Radius);
            }

            //The offset could be also dynamically determined (using the mesh intersection distance or using Physics Compute Pentration in unity)

            this.SkeletonAccess.SetChannelData(currentPosture);

            //Get the shoulder positions
            MVector3 leftShoulderPosition  = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.LeftShoulder);
            MVector3 rightShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.RightShoulder);

            //Compute the direction vector pointing from the avatar towards the respective hand
            MVector3 dir = new MVector3(0, 0, 0);

            switch (this.handJoint)
            {
            case MJointType.LeftWrist:
                dir = leftShoulderPosition.Subtract(rightShoulderPosition).Normalize();
                break;

            case MJointType.RightWrist:
                dir = rightShoulderPosition.Subtract(leftShoulderPosition).Normalize();
                break;
            }

            //Add an offset on top of the position
            return(targetHandPosition.Add(dir.Multiply(offset)));
        }
Exemplo n.º 6
0
        /// <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);
        }
Exemplo n.º 7
0
        public MVector3 GetGlobalPosition()
        {
            if (this.globalPos == null)
            {
                MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
                MVector3    parentPosition = new MVector3(0, 0, 0);
                if (this.parentJoint != null)
                {
                    parentRotation = this.parentJoint.globalRot;
                    parentPosition = this.parentJoint.globalPos;
                }


                MVector3 rotatedOffset      = parentRotation.Multiply(this.joint.Position);
                MVector3 rotatedTranslation = parentRotation.Multiply(this.joint.Rotation).Multiply(this.translation);

                this.globalPos = rotatedTranslation.Add(rotatedOffset).Add(parentPosition);
            }
            return(this.globalPos);
        }
Exemplo n.º 8
0
        /// <summary>
        /// Rotates the current transform around the specific point and axis
        /// </summary>
        /// <param name="center">The rotation center</param>
        /// <param name="axis">The rotation axis</param>
        /// <param name="angle">The angle to rotate</param>
        private static MTransform RotateAround(MTransform transform, MVector3 center, MVector3 axis, float angle)
        {
            MTransform res = new MTransform()
            {
                ID = System.Guid.NewGuid().ToString()
            };

            MVector3 pos = transform.Position;

            MQuaternion rot = MQuaternionExtensions.FromEuler(axis.Multiply(angle)); // get the desired rotation
            MVector3    dir = pos.Subtract(center);                                  // find current direction relative to center

            dir = rot.Multiply(dir);                                                 // rotate the direction

            res.Position = center.Add(dir);                                          // define new position
            MQuaternion myRot = transform.Rotation;

            res.Rotation = transform.Rotation.Multiply(MQuaternionExtensions.Inverse(myRot).Multiply(rot).Multiply(myRot));


            return(res);
        }
Exemplo n.º 9
0
        /// <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);
        }
Exemplo n.º 10
0
        private int SetAvatarPostureValues(MAvatarPostureValues values, int id, List <MJointType> animatedJoints)
        {
            if (animatedJoints == null || animatedJoints.Contains(this.joint.Type))
            {
                translation = new MVector3(0, 0, 0);

                this.currentRotationValues = new MQuaternion(0, 0, 0, 1);

                if (values != null)
                {
                    MVector3 translation = new MVector3(0, 0, 0);
                    for (int i = 0; i < this.joint.Channels.Count; i++)
                    {
                        switch (this.joint.Channels[i])
                        {
                        case MChannel.WRotation:
                            this.currentRotationValues.W = values.PostureData[id];
                            break;

                        case MChannel.XRotation:
                            this.currentRotationValues.X = values.PostureData[id];
                            break;

                        case MChannel.YRotation:
                            this.currentRotationValues.Y = values.PostureData[id];
                            break;

                        case MChannel.ZRotation:
                            this.currentRotationValues.Z = values.PostureData[id];
                            break;

                        case MChannel.XOffset:
                            translation.X = (float)values.PostureData[id];
                            break;

                        case MChannel.YOffset:
                            translation.Y = (float)values.PostureData[id];
                            break;

                        case MChannel.ZOffset:
                            translation.Z = (float)values.PostureData[id];
                            break;
                        }
                        id += 1;
                    }

                    this.translation = translation;
                }
            }



            MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
            MVector3    parentPosition = new MVector3(0, 0, 0);

            if (this.parentJoint != null)
            {
                parentRotation = this.parentJoint.globalRot;
                parentPosition = this.parentJoint.globalPos;
            }


            MVector3 rotatedOffset      = parentRotation.Multiply(this.joint.Position);
            MVector3 rotatedTranslation = parentRotation.Multiply(this.joint.Rotation).Multiply(this.translation);

            this.globalPos = rotatedTranslation.Add(rotatedOffset).Add(parentPosition);

            this.globalRot = parentRotation.Multiply(this.joint.Rotation).Multiply(this.currentRotationValues);


            foreach (Joint c in this.children)
            {
                id = c.SetAvatarPostureValues(values, id, animatedJoints);
            }
            return(id);
        }
Exemplo n.º 11
0
        /// <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);
        }
Exemplo n.º 12
0
        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);
        }