/// <summary>
 /// Constructs a new distance joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="anchorA">Anchor point on the first bone in world space.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space.</param>
 public IKDistanceJoint(Bone connectionA, Bone connectionB, Vector3 anchorA, Vector3 anchorB)
     : base(connectionA, connectionB)
 {
     AnchorA = anchorA;
     AnchorB = anchorB;
     Vector3.Distance(ref anchorA, ref anchorB, out distance);
 }
Beispiel #2
0
 /// <summary>
 /// Constructs a new distance joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="anchorA">Anchor point on the first bone in world space.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space.</param>
 public IKDistanceJoint(Bone connectionA, Bone connectionB, System.Numerics.Vector3 anchorA, System.Numerics.Vector3 anchorB)
     : base(connectionA, connectionB)
 {
     AnchorA = anchorA;
     AnchorB = anchorB;
     Vector3Ex.Distance(ref anchorA, ref anchorB, out distance);
 }
 /// <summary>
 /// Constructs a new point on line joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="lineAnchor">Anchor point of the line attached to the first bone in world space.</param>
 /// <param name="lineDirection">Direction of the line attached to the first bone in world space. Must be unit length.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space which tries to stay on connection A's line.</param>
 public IKPointOnLineJoint(Bone connectionA, Bone connectionB, System.Numerics.Vector3 lineAnchor, System.Numerics.Vector3 lineDirection, System.Numerics.Vector3 anchorB)
     : base(connectionA, connectionB)
 {
     LineAnchor = lineAnchor;
     LineDirection = lineDirection;
     AnchorB = anchorB;
 }
Beispiel #4
0
 /// <summary>
 /// Builds a new swing limit. Prevents two bones from rotating beyond a certain angle away from each other as measured by attaching an axis to each connected bone.
 /// </summary>
 /// <param name="connectionA">First connection of the limit.</param>
 /// <param name="connectionB">Second connection of the limit.</param>
 /// <param name="axisA">Axis attached to connectionA in world space.</param>
 /// <param name="axisB">Axis attached to connectionB in world space.</param>
 /// <param name="maximumAngle">Maximum angle allowed between connectionA's axis and connectionB's axis.</param>
 public IKSwingLimit(Bone connectionA, Bone connectionB, System.Numerics.Vector3 axisA, System.Numerics.Vector3 axisB, float maximumAngle)
     : base(connectionA, connectionB)
 {
     AxisA = axisA;
     AxisB = axisB;
     MaximumAngle = maximumAngle;
 }
Beispiel #5
0
 /// <summary>
 /// Constructs a new distance joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="anchorA">Anchor point on the first bone in world space.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space.</param>
 public IKDistanceJoint(Bone connectionA, Bone connectionB, ref Vector3 anchorA, ref Vector3 anchorB)
     : base(connectionA, connectionB)
 {
     SetAnchorA(ref anchorA);
     SetAnchorB(ref anchorB);
     Vector3.Distance(ref anchorA, ref anchorB, out distance);
 }
        void BuildStick(Vector3 position, int linkCount, out List<Bone> bones, out List<Entity> boneEntities)
        {
            //Set up a bone chain.
            bones = new List<Bone>();
            boneEntities = new List<Entity>();
            var previousBoneEntity = new Cylinder(position, 1, .2f);
            var previousBone = new Bone(previousBoneEntity.Position, previousBoneEntity.Orientation, previousBoneEntity.Radius, previousBoneEntity.Height);
            bones.Add(previousBone);
            boneEntities.Add(previousBoneEntity);

            
            for (int i = 1; i < linkCount; i++)
            {
                var boneEntity = new Cylinder(previousBone.Position + new Vector3(0, 1, 0), 1, .2f);
                var bone = new Bone(boneEntity.Position, boneEntity.Orientation, boneEntity.Radius, boneEntity.Height);
                bones.Add(bone);
                boneEntities.Add(boneEntity);

                //Make a relationship between the two bones and entities.
                CollisionRules.AddRule(previousBoneEntity, boneEntity, CollisionRule.NoBroadPhase);
                Vector3 anchor = (previousBoneEntity.Position + boneEntity.Position) / 2;
                //var dynamicsBallSocketJoint = new BallSocketJoint(previousBoneEntity, boneEntity, anchor);
                //var dynamicsAngularFriction = new NoRotationJoint(previousBoneEntity, boneEntity);
                //Space.Add(dynamicsBallSocketJoint);
                //Space.Add(dynamicsAngularFriction);
                var ballSocket = new IKBallSocketJoint(previousBone, bone, anchor);
                var angularJoint = new IKAngularJoint(previousBone, bone);

   
                previousBone = bone;
                previousBoneEntity = boneEntity;
            }
        }
 /// <summary>
 /// Constructs a new point on line joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="lineAnchor">Anchor point of the line attached to the first bone in world space.</param>
 /// <param name="lineDirection">Direction of the line attached to the first bone in world space. Must be unit length.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space which tries to stay on connection A's line.</param>
 public IKPointOnLineJoint(Bone connectionA, Bone connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 anchorB)
     : base(connectionA, connectionB)
 {
     LineAnchor = lineAnchor;
     LineDirection = lineDirection;
     AnchorB = anchorB;
 }
 /// <summary>
 /// Constructs a new point on plane joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="planeAnchor">Anchor point of the plane attached to the first bone in world space.</param>
 /// <param name="planeNormal">Normal of the plane attached to the first bone in world space. Must be unit length.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space which is measured against the other connection's anchor.</param>
 public IKPointOnPlaneJoint(Bone connectionA, Bone connectionB, Vector3 planeAnchor, Vector3 planeNormal, Vector3 anchorB)
     : base(connectionA, connectionB)
 {
     PlaneAnchor = planeAnchor;
     PlaneNormal = planeNormal;
     AnchorB = anchorB;
 }
 StateControl GetControl(Bone bone)
 {
     var control = stateControlsPool.Count > 0 ? stateControlsPool.Pop() : new StateControl();
     control.TargetBone = bone;
     Controls.Add(control);
     return control;
 }
 /// <summary>
 /// Constructs a new distance joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="anchorA">Anchor point on the first bone in world space.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space.</param>
 /// <param name="minimumDistance">Minimum distance that the joint connections should be kept from each other.</param>
 /// <param name="maximumDistance">Maximum distance that the joint connections should be kept from each other.</param>
 public IKDistanceLimit(Bone connectionA, Bone connectionB, Vector3 anchorA, Vector3 anchorB, float minimumDistance, float maximumDistance)
     : base(connectionA, connectionB)
 {
     AnchorA = anchorA;
     AnchorB = anchorB;
     MinimumDistance = minimumDistance;
     MaximumDistance = maximumDistance;
 }
Beispiel #11
0
        /// <summary>
        /// Builds a new twist limit. Prevents two bones from rotating beyond a certain angle away from each other as measured by attaching an axis to each connected bone.
        /// </summary>
        /// <param name="connectionA">First connection of the limit.</param>
        /// <param name="connectionB">Second connection of the limit.</param>
        /// <param name="axisA">Axis attached to connectionA in world space.</param>
        /// <param name="axisB">Axis attached to connectionB in world space.</param>
        public IKTwistJoint(Bone connectionA, Bone connectionB, Vector3 axisA, Vector3 axisB)
            : base(connectionA, connectionB)
        {
            AxisA = axisA;
            AxisB = axisB;

            ComputeMeasurementAxes();
        }
Beispiel #12
0
 /// <summary>
 /// Constructs a new distance joint.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="anchorA">Anchor point on the first bone in world space.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space.</param>
 /// <param name="minimumDistance">Minimum distance that the joint connections should be kept from each other.</param>
 /// <param name="maximumDistance">Maximum distance that the joint connections should be kept from each other.</param>
 public IKDistanceLimit(Bone connectionA, Bone connectionB, Vector3 anchorA, Vector3 anchorB, float minimumDistance, float maximumDistance)
     : base(connectionA, connectionB)
 {
     SetAnchorA(ref anchorA);
     SetAnchorB(ref anchorB);
     MinimumDistance = minimumDistance;
     MaximumDistance = maximumDistance;
 }
 /// <summary>
 /// Constructs a 3DOF angular joint which tries to keep two bones in angular alignment.
 /// </summary>
 /// <param name="connectionA">First bone to connect to the joint.</param>
 /// <param name="connectionB">Second bone to connect to the joint.</param>
 public IKAngularJoint(Bone connectionA, Bone connectionB)
     : base(connectionA, connectionB)
 {
     Quaternion orientationAConjugate;
     Quaternion.Conjugate(ref ConnectionA.Orientation, out orientationAConjugate);
     //Store the orientation from A to B in A's local space in the GoalRelativeOrientation.
     Quaternion.Concatenate(ref ConnectionB.Orientation, ref orientationAConjugate, out GoalRelativeOrientation);
 }
 /// <summary>
 /// Constructs a new axis limit.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="lineAnchor">Anchor point of the line attached to the first bone in world space.</param>
 /// <param name="lineDirection">Direction of the line attached to the first bone in world space. Must be unit length.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space which is measured against the other connection's anchor.</param>
 /// <param name="minimumDistance">Minimum distance that the joint connections should be kept from each other along the axis.</param>
 /// <param name="maximumDistance">Maximum distance that the joint connections should be kept from each other along the axis.</param>
 public IKLinearAxisLimit(Bone connectionA, Bone connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 anchorB, float minimumDistance, float maximumDistance)
     : base(connectionA, connectionB)
 {
     LineAnchor = lineAnchor;
     LineDirection = lineDirection;
     AnchorB = anchorB;
     MinimumDistance = minimumDistance;
     MaximumDistance = maximumDistance;
 }
Beispiel #15
0
        /// <summary>
        /// Builds a new twist limit. Prevents two bones from rotating beyond a certain angle away from each other as measured by attaching an axis to each connected bone.
        /// </summary>
        /// <param name="connectionA">First connection of the limit.</param>
        /// <param name="connectionB">Second connection of the limit.</param>
        /// <param name="axisA">Axis attached to connectionA in world space.</param>
        /// <param name="axisB">Axis attached to connectionB in world space.</param>
        /// <param name="maximumAngle">Maximum angle allowed between connectionA's axis and connectionB's axis.</param>
        public IKTwistLimit(Bone connectionA, Bone connectionB, Vector3 axisA, Vector3 axisB, float maximumAngle)
            : base(connectionA, connectionB)
        {
            AxisA = axisA;
            AxisB = axisB;
            MaximumAngle = maximumAngle;

            ComputeMeasurementAxes();
        }
Beispiel #16
0
 /// <summary>
 /// Builds a ball socket joint.
 /// </summary>
 /// <param name="connectionA">First connection in the pair.</param>
 /// <param name="connectionB">Second connection in the pair.</param>
 /// <param name="anchor">World space anchor location used to initialize the local anchors.</param>
 public IKBallSocketJoint(Bone connectionA, Bone connectionB, Vector3 anchor)
     : base(connectionA, connectionB)
 {
     Vector3 tmp;
     anchor.Sub(ref ConnectionA.Position, out tmp);
     SetOffsetA(ref tmp);
     anchor.Sub(ref ConnectionB.Position, out tmp);
     SetOffsetB(ref tmp);
 }
 public void TryToAddBone(Bone bone, Vector3 grabbedLocation)
 {
     bool alreadyConstrainingBone = false;
     for (int i = 0; i < stateControls.Count; i++)
     {
         var entry = stateControls[i];
         entry.GrabOffset = grabbedLocation - entry.Control.TargetBone.Position;
         stateControls[i] = entry;
         if (entry.Control.TargetBone == bone)
         {
             alreadyConstrainingBone = true;
         }
     }
     if (!alreadyConstrainingBone)
     {
         //Add a new control to the group for this bone.
         var entry = new ControlEntry { Control = GetControl(bone), GrabOffset = grabbedLocation - bone.Position };
         stateControls.Add(entry);
     }
     distanceToTarget = Vector3.Dot(camera.WorldMatrix.Forward, grabbedLocation - camera.Position);
 }
Beispiel #18
0
 void NotifyPredecessorsOfCycle(Bone bone)
 {
     //Rather than attempting to only mark cycles, this will simply mark all of the cycle elements and any cycle predecessors up to the unstressed root.
     if (!bone.unstressedCycle && bone.stressCount == 0)
     {
         bone.unstressedCycle = true;
         foreach (var predecessor in bone.predecessors)
         {
             NotifyPredecessorsOfCycle(predecessor);
         }
     }
 }
 /// <summary>
 /// Builds a ball socket joint.
 /// </summary>
 /// <param name="connectionA">First connection in the pair.</param>
 /// <param name="connectionB">Second connection in the pair.</param>
 /// <param name="anchor">World space anchor location used to initialize the local anchors.</param>
 public IKBallSocketJoint(Bone connectionA, Bone connectionB, System.Numerics.Vector3 anchor)
     : base(connectionA, connectionB)
 {
     OffsetA = anchor - ConnectionA.Position;
     OffsetB = anchor - ConnectionB.Position;
 }
Beispiel #20
0
 protected IKJoint(Bone connectionA, Bone connectionB)
 {
     ConnectionA = connectionA;
     ConnectionB = connectionB;
     Enabled = true;
 }
        void BuildJointTest(Vector3 position)
        {
            Bone a, b;
            a = new Bone(position + new Vector3(0, 5, 0), Quaternion.Identity, .5f, 1);
            b = new Bone(position + new Vector3(0, 7, 0), Quaternion.Identity, .5f, 1);
            //var ikJoint = new IKBallSocketJoint(a, b, (a.Position + b.Position) * 0.5f);
            //var ikLimit = new IKSwingLimit(a, b, Vector3.Up, Vector3.Up, MathHelper.PiOver2);
            //var ikRevolute = new IKRevoluteJoint(a, b, Vector3.Right);
            //var ikSwivelHingeJoint = new IKSwivelHingeJoint(a, b, Vector3.Right, Vector3.Up);
            //var ikAngularJoint = new IKAngularJoint(a, b);
            //var ikTwistLimit = new IKTwistLimit(a, b, Vector3.Up, Vector3.Up, MathHelper.PiOver2);
            //var ikDistanceLimit = new IKDistanceLimit(a, b, a.Position + new Vector3(0, 0.5f, 0), b.Position + new Vector3(0, -0.5f, 0), 1f, 4);
            //var ikLinearAxisLimit = new IKLinearAxisLimit(a, b, a.Position + new Vector3(0, 0.5f, 0), Vector3.Up, b.Position + new Vector3(0, -0.5f, 0), 0, 4);
            //var ikTwistJoint = new IKTwistJoint(a, b, Vector3.Up, Vector3.Up);
            //var ikPointOnLineJoint = new IKPointOnLineJoint(a, b, a.Position + new Vector3(0, 0.5f, 0), Vector3.Up, b.Position - new Vector3(0, 0.5f, 0));
            var ikPointOnPlaneJoint = new IKPointOnPlaneJoint(a, b, a.Position + new Vector3(0, 1f, 0), Vector3.Up, b.Position - new Vector3(0, 1f, 0));

            //ikPointOnLineJoint.Softness = 0;
            //ikPointOnLineJoint.ErrorCorrectionFactor = 0;
            //solver.VelocitySubiterationCount = 10;

            var entityA = new Cylinder(a.Position, 1, 0.5f, 10);
            var entityB = new Cylinder(b.Position, 1, 0.5f, 10);
            entityB.Orientation = b.Orientation;
            //var joint = new BallSocketJoint(entityA, entityB, (a.Position + b.Position) * 0.5f);
            //var limit = new SwingLimit(entityA, entityB, Vector3.Up, Vector3.Up, MathHelper.PiOver2);
            //var revolute = new RevoluteAngularJoint(entityA, entityB, Vector3.Right);
            //var swivelHingeJoint = new SwivelHingeAngularJoint(entityA, entityB, Vector3.Right, Vector3.Up);
            //var angularJoint = new NoRotationJoint(entityA, entityB);
            //var twistLimit = new TwistLimit(entityA, entityB, Vector3.Up, Vector3.Up, -MathHelper.PiOver2, MathHelper.PiOver2);
            //var distanceLimit = new DistanceLimit(entityA, entityB, ikDistanceLimit.AnchorA, ikDistanceLimit.AnchorB, ikDistanceLimit.MinimumDistance, ikDistanceLimit.MaximumDistance);
            //var linearAxisLimit = new LinearAxisLimit(entityA, entityB, ikLinearAxisLimit.LineAnchor, ikLinearAxisLimit.AnchorB, ikLinearAxisLimit.LineDirection, ikLinearAxisLimit.MinimumDistance, ikLinearAxisLimit.MaximumDistance);
            //var twistJoint = new TwistJoint(entityA, entityB, Vector3.Up, Vector3.Up);
            //var pointOnLineJoint = new PointOnLineJoint(entityA, entityB, ikPointOnLineJoint.LineAnchor, ikPointOnLineJoint.LineDirection, ikPointOnLineJoint.AnchorB);
            var pointOnPlaneJoint = new PointOnPlaneJoint(entityA, entityB, ikPointOnPlaneJoint.PlaneAnchor, ikPointOnPlaneJoint.PlaneNormal, ikPointOnPlaneJoint.AnchorB);

            Space.Add(entityA);
            Space.Add(entityB);
            //Space.Add(joint);
            //Space.Add(limit);
            //Space.Add(revolute);
            //Space.Add(swivelHingeJoint);
            //Space.Add(angularJoint);
            //Space.Add(twistLimit);
            //Space.Add(distanceLimit);
            //Space.Add(linearAxisLimit);
            //Space.Add(twistJoint);
            //Space.Add(pointOnLineJoint);
            Space.Add(pointOnPlaneJoint);
            bones.Add(new BoneRelationship(a, entityA));
            bones.Add(new BoneRelationship(b, entityB));
        }
        void BuildRing(Vector3 position)
        {
            int incrementCount = 20;
            float radius = 5;
            float anglePerIncrement = MathHelper.TwoPi / incrementCount;
            Bone[] bonesList = new Bone[incrementCount];
            for (int i = 0; i < incrementCount; i++)
            {
                Vector3 bonePosition;
            #if !WINDOWS
                bonePosition = new Vector3();
            #endif
                bonePosition.X = (float)Math.Cos(anglePerIncrement * i);
                bonePosition.Y = 0;
                bonePosition.Z = (float)Math.Sin(anglePerIncrement * i);
                bonePosition = bonePosition * radius + position;
                bonesList[i] = new Bone(bonePosition,
                                     Quaternion.Concatenate(Quaternion.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2), Quaternion.CreateFromAxisAngle(Vector3.Up, -anglePerIncrement * i)),
                                     0.5f,
                                     MathHelper.Pi * radius * 2 / incrementCount);
            }

            for (int i = 0; i < bonesList.Length; i++)
            {
                var boneA = bonesList[i];
                var boneB = bonesList[(i + 1) % incrementCount];
                var upA = Quaternion.Transform(Vector3.Up, boneA.Orientation);
                var upB = Quaternion.Transform(Vector3.Up, boneB.Orientation);
                joints.Add(new IKBallSocketJoint(boneA, boneB, (boneA.Position + upA * boneB.HalfHeight + boneB.Position - upB * boneB.HalfHeight) * .5f));
                joints.Add(new IKSwingLimit(boneA, boneB, upA, upB, MathHelper.Pi * .5f));
            }
            Cylinder[] boneEntitiesList = new Cylinder[incrementCount];
            for (int i = 0; i < incrementCount; i++)
            {
                var boneEntity = new Cylinder(new MotionState { Position = bonesList[i].Position, Orientation = bonesList[i].Orientation }, bonesList[i].Height, bonesList[i].Radius, 10);
                bones.Add(new BoneRelationship(bonesList[i], boneEntity));
                boneEntitiesList[i] = boneEntity;
                Space.Add(boneEntity);
            }

            for (int i = 0; i < incrementCount; i++)
            {
                var boneA = boneEntitiesList[i];
                var boneB = boneEntitiesList[(i + 1) % incrementCount];
                var upA = Quaternion.Transform(Vector3.Up, boneA.Orientation);
                var upB = Quaternion.Transform(Vector3.Up, boneB.Orientation);
                var joint = new BallSocketJoint(boneA, boneB, (boneA.Position + upA * boneB.Height * 0.5f + boneB.Position - upB * boneB.Height * 0.5f) * .5f);
                var swingLimit = new SwingLimit(boneA, boneB, upA, upB, MathHelper.Pi * .5f);
                Space.Add(swingLimit);
                Space.Add(joint);
                CollisionRules.AddRule(boneA, boneB, CollisionRule.NoBroadPhase);
            }
        }
        void BuildRoboArmThing(Vector3 position)
        {
            //Make the IK representation
            Bone baseBone = new Bone(position, Quaternion.Identity, 1, 3);
            Bone upperArm = new Bone(baseBone.Position + new Vector3(0, 1.5f + 2, 0), Quaternion.Identity, .4f, 4);
            Bone lowerArm = new Bone(upperArm.Position + new Vector3(0, 2 + 1, 0), Quaternion.Identity, .7f, 2);
            Bone bonkDevice = new Bone(lowerArm.Position + new Vector3(0, 5, 0), Quaternion.Identity, .6f, 1.2f);

            joints.Add(new IKBallSocketJoint(baseBone, upperArm, baseBone.Position + new Vector3(0, 1.5f, 0)));
            joints.Add(new IKSwingLimit(baseBone, upperArm, Vector3.Up, Vector3.Up, MathHelper.PiOver4));
            joints.Add(new IKBallSocketJoint(upperArm, lowerArm, upperArm.Position + new Vector3(0, 2f, 0)));
            joints.Add(new IKRevoluteJoint(upperArm, lowerArm, Vector3.Forward));
            joints.Add(new IKSwingLimit(upperArm, lowerArm, Vector3.Up, Vector3.Up, MathHelper.PiOver4));
            joints.Add(new IKPointOnLineJoint(lowerArm, bonkDevice, lowerArm.Position, Vector3.Up, bonkDevice.Position));
            joints.Add(new IKAngularJoint(lowerArm, bonkDevice));
            joints.Add(new IKLinearAxisLimit(lowerArm, bonkDevice, lowerArm.Position, Vector3.Up, bonkDevice.Position, 1.6f, 5));

            //Make the dynamics representation
            Entity baseEntity = new Cylinder(baseBone.Position, baseBone.Height, baseBone.Radius, 10);
            Entity upperArmEntity = new Cylinder(upperArm.Position, upperArm.Height, upperArm.Radius, 7);
            Entity lowerArmEntity = new Cylinder(lowerArm.Position, lowerArm.Height, lowerArm.Radius, 5);
            Entity bonkDeviceEntity = new Cylinder(bonkDevice.Position, bonkDevice.Height, bonkDevice.Radius, 3);
            bonkDeviceEntity.Orientation = bonkDevice.Orientation;

            Space.Add(baseEntity);
            Space.Add(upperArmEntity);
            Space.Add(lowerArmEntity);
            Space.Add(bonkDeviceEntity);

            Space.Add(new BallSocketJoint(baseEntity, upperArmEntity, baseBone.Position + new Vector3(0, 1.5f, 0)));
            Space.Add(new SwingLimit(baseEntity, upperArmEntity, Vector3.Up, Vector3.Up, MathHelper.PiOver4));
            Space.Add(new BallSocketJoint(upperArmEntity, lowerArmEntity, upperArm.Position + new Vector3(0, 2f, 0)));
            Space.Add(new RevoluteAngularJoint(upperArmEntity, lowerArmEntity, Vector3.Forward));
            Space.Add(new SwingLimit(upperArmEntity, lowerArmEntity, Vector3.Up, Vector3.Up, MathHelper.PiOver4));
            Space.Add(new PointOnLineJoint(lowerArmEntity, bonkDeviceEntity, lowerArm.Position, Vector3.Up, bonkDevice.Position));
            var motor = new AngularMotor(lowerArmEntity, bonkDeviceEntity);
            motor.Settings.Mode = MotorMode.Servomechanism;
            Space.Add(motor);
            Space.Add(new LinearAxisLimit(lowerArmEntity, bonkDeviceEntity, lowerArm.Position, bonkDevice.Position, Vector3.Up, 1.6f, 5));

            CollisionRules.AddRule(baseEntity, upperArmEntity, CollisionRule.NoBroadPhase);
            CollisionRules.AddRule(upperArmEntity, lowerArmEntity, CollisionRule.NoBroadPhase);
            CollisionRules.AddRule(lowerArmEntity, bonkDeviceEntity, CollisionRule.NoBroadPhase);

            //Relate the two!
            bones.Add(new BoneRelationship(baseBone, baseEntity));
            bones.Add(new BoneRelationship(upperArm, upperArmEntity));
            bones.Add(new BoneRelationship(lowerArm, lowerArmEntity));
            bones.Add(new BoneRelationship(bonkDevice, bonkDeviceEntity));
        }
 /// <summary>
 /// Constructs a new orientation joint.
 /// Orientation joints can be used to simulate the angular portion of a hinge.
 /// Orientation joints allow rotation around only a single axis.
 /// </summary>
 /// <param name="connectionA">First entity connected in the orientation joint.</param>
 /// <param name="connectionB">Second entity connected in the orientation joint.</param>
 /// <param name="freeAxis">Axis allowed to rotate freely in world space.</param>
 public IKRevoluteJoint(Bone connectionA, Bone connectionB, Vector3 freeAxis)
     : base(connectionA, connectionB)
 {
     WorldFreeAxisA = freeAxis;
     WorldFreeAxisB = freeAxis;
 }
Beispiel #25
0
 protected IKLimit(Bone connectionA, Bone connectionB)
     : base(connectionA, connectionB)
 {
 }
Beispiel #26
0
        void NotifyPredecessorsOfStress(Bone bone)
        {
            //We don't need to tell already-stressed bones about the fact that they are stressed.
            //Their predecessors are already stressed either by previous notifications like this or
            //through the predecessors being added on after the fact and seeing that the path was stressed.
            if (!bone.traversed)
            {
                bone.traversed = true;
                bone.stressCount++;
                foreach (var predecessor in bone.predecessors)
                {

                    NotifyPredecessorsOfStress(predecessor);
                }
            }
        }
Beispiel #27
0
 /// <summary>
 /// Constructs a new axis limit.
 /// </summary>
 /// <param name="connectionA">First bone connected by the joint.</param>
 /// <param name="connectionB">Second bone connected by the joint.</param>
 /// <param name="lineAnchor">Anchor point of the line attached to the first bone in world space.</param>
 /// <param name="lineDirection">Direction of the line attached to the first bone in world space. Must be unit length.</param>
 /// <param name="anchorB">Anchor point on the second bone in world space which is measured against the other connection's anchor.</param>
 /// <param name="minimumDistance">Minimum distance that the joint connections should be kept from each other along the axis.</param>
 /// <param name="maximumDistance">Maximum distance that the joint connections should be kept from each other along the axis.</param>
 public IKLinearAxisLimit(Bone connectionA, Bone connectionB, ref Vector3 lineAnchor, ref Vector3 lineDirection, ref Vector3 anchorB, float minimumDistance, float maximumDistance)
     : base(connectionA, connectionB)
 {
     SetLineAnchor(ref lineAnchor);
     LineDirection = lineDirection;
     SetAnchorB(ref anchorB);
     MinimumDistance = minimumDistance;
     MaximumDistance = maximumDistance;
 }
Beispiel #28
0
        void DistributeMass(Bone bone)
        {
            //Accumulate the number of child joints which we are going to distribute mass to.
            foreach (var joint in bone.joints)
            {
                Bone boneToAnalyze = joint.ConnectionA == bone ? joint.ConnectionB : joint.ConnectionA;

                if (boneToAnalyze.traversed || boneToAnalyze.unstressedCycle ||
                    uniqueChildren.Contains(boneToAnalyze)) //There could exist multiple joints involved with the same pair of bones; don't continually double count.
                {
                    //The bone was already visited or was a member of the stressed path we branched from. Do not proceed.
                    continue;
                }
                uniqueChildren.Add(boneToAnalyze);
            }
            //We distribute a portion of the current bone's total mass to the child bones.
            //By applying a multiplier automassUnstressedFalloff, we guarantee that a chain has a certain maximum weight (excluding cycles).
            //This is thanks to the convergent geometric series sum(automassUnstressedFalloff^n, 1, infinity).
            float massPerChild = uniqueChildren.Count > 0 ? automassUnstressedFalloff * bone.Mass / uniqueChildren.Count : 0;

            uniqueChildren.Clear();
            //(If the number of children is 0, then the only bones which can exist are either bones which were already traversed and will be skipped
            //or bones which are members of unstressed cycles and will inherit the full parent weight. Don't have to worry about the 0 mass.)

            //The current bone is known to not be stressed.
            foreach (var joint in bone.joints)
            {
                Bone boneToAnalyze = joint.ConnectionA == bone ? joint.ConnectionB : joint.ConnectionA;
                //Note that no testing for pinned bones is necessary; based on the previous stressed path searches,
                //any unstressed bone is known to not be a path to any pinned bones.
                if (boneToAnalyze.traversed)// || bone.unstressedCycle)//bone.predecessors.Contains(boneToAnalyze))
                {
                    //The bone was already visited or was a member of the stressed path we branched from. Do not proceed.
                    continue;
                }

                if (boneToAnalyze.unstressedCycle)
                {
                    //This bone is part of a cycle! We cannot give it less mass; that would add in a potential instability.
                    //Just give it the current node's full mass.
                    boneToAnalyze.Mass = bone.Mass;
                }
                else
                {
                    //This bone is not a part of a cycle; give it the allotted mass.
                    boneToAnalyze.Mass = massPerChild;
                }
                //The root bone is already added to the traversal set; add the children.
                boneToAnalyze.traversed = true;
                //Note that we do not need to add anything to the bones list here; the previous FindCycles DFS on this unstressed part of the graph did it for us.
                DistributeMass(boneToAnalyze);

            }

        }
Beispiel #29
0
        void FindCycles(Bone bone)
        {
            //The current bone is known to not be stressed.
            foreach (var joint in bone.joints)
            {
                Bone boneToAnalyze = joint.ConnectionA == bone ? joint.ConnectionB : joint.ConnectionA;

                if (bone.predecessors.Contains(boneToAnalyze) || //Do not attempt to traverse a path which leads to this bone.
                    boneToAnalyze.predecessors.Contains(bone)) //Do not attempt to traverse a path which was already traversed *from this bone.*
                    continue;
                //We found this bone. Regardless of what happens after, make sure that the bone knows about this path.
                boneToAnalyze.predecessors.Add(bone);

                if (boneToAnalyze.IsActive)
                {
                    //This bone is butting up against a node which was previously visited.
                    //Based on the previous stress path computation, there is only one entry point into an unstressed part of the graph.
                    //by the previous condition, we know it's not our immediate parent. We hit an unstressed part of the graph.

                    //In other words, this is an unstressed cycle.

                    //Rather than attempting to only mark cycles, this will simply mark all of the cycle elements and any cycle predecessors up to the unstressed root.
                    NotifyPredecessorsOfCycle(bone);
                    continue;
                }
                //Note that no testing for pinned bones is necessary; based on the previous stressed path searches,
                //any unstressed bone is known to not be a path to any pinned bones.

                //The root bone is already added to the active set by the parent breadth-first search.
                //Children are added to the active set.
                boneToAnalyze.IsActive = true;
                bones.Add(boneToAnalyze);
                FindCycles(boneToAnalyze);
            }
        }
Beispiel #30
0
        void FindStressedPaths(Bone bone)
        {
            bone.IsActive = true; //We must keep track of which bones have been visited
            bones.Add(bone);
            foreach (var joint in bone.joints)
            {
                Bone boneToAnalyze = joint.ConnectionA == bone ? joint.ConnectionB : joint.ConnectionA;
                if (bone.predecessors.Contains(boneToAnalyze) ||  //boneToAnalyze is a parent of bone. Don't revisit them, that's where we came from!
                    boneToAnalyze.predecessors.Contains(bone)) //This bone already explored the next bone; don't do it again.
                    continue;

                if (!boneToAnalyze.Pinned)
                {
                    //The boneToAnalyze is reached by following a path from bone. We record this regardless of whether or not we traverse further.
                    //There is one exception: DO NOT create paths to pinned bones!
                    boneToAnalyze.predecessors.Add(bone);
                }

                if (boneToAnalyze.Pinned || boneToAnalyze.traversed)
                {
                    //This bone is connected to a pinned bone (or a bone which is directly or indirectly connected to a pinned bone)!
                    //This bone and all of its predecessors are a part of a 'stressed path.'
                    //This backwards notification is necessary because this depth first search could attempt a deep branch which winds 
                    //its way back up to a part of the graph which SHOULD be marked as stressed, but is not yet marked because this path
                    //has not popped its way all the way up the stack yet! Left untreated, this would lead to missed stressed paths.
                    NotifyPredecessorsOfStress(bone);
                    continue;
                }

                if (boneToAnalyze.targetedByOtherControl)
                {
                    //We will consider other controls to be sources of stress. This prevents mass ratio issues from allowing multiple controls to tear a structure apart.
                    //We do not, however, stop the traversal here. Allow it to continue.         
                    NotifyPredecessorsOfStress(bone);
                }
                if (boneToAnalyze.IsActive)
                {
                    //The bone has already been visited. We should not proceed.
                    //Any bone which is visited but not stressed is either A: not fully explored yet or B: fully explored.
                    //Given that we followed an unexplored path to the bone, it must be not fully explored.
                    //However, we do not attempt to perform exploration on the bone: any not-yet-fully-explored bones
                    //must belong to one of our parents in the DFS! They will take care of it.
                    continue;
                }

                //The search hasn't yet found a stressed path or pinned bone yet.
                //Keep on movin' on!
                FindStressedPaths(boneToAnalyze);
                //If a child finds a pin, we will be notified of that fact by the above while loop which traverses the parent pointers.
            }


        }