/// <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); }
/// <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; }
/// <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; }
/// <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; }
/// <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(); }
/// <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; }
/// <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(); }
/// <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); }
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; }
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; }
protected IKLimit(Bone connectionA, Bone connectionB) : base(connectionA, connectionB) { }
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); } } }
/// <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; }
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); } }
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); } }
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. } }