/// <summary> /// Analytic solutions useful fo hands or feet, all in local model space /// </summary> /// <param name="desiredEnd">in local model space</param> /// <param name="firstBone"></param> /// <param name="secondBone"></param> /// <param name="finalTransform"></param> /// <param name="finalBone"></param> /// <param name="allowFinalBoneTranslation"></param> /// <returns></returns> public static bool SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true) { Matrix firstBoneAbsoluteTransform = firstBone.AbsoluteTransform; Matrix secondBoneAbsoluteTransform = secondBone.AbsoluteTransform; Matrix endBoneAbsoluteTransform = endBone.AbsoluteTransform; Vector3 origin = firstBoneAbsoluteTransform.Translation; Vector3 originToCurrentEnd = endBoneAbsoluteTransform.Translation - origin; Vector3 originToDesiredEnd = desiredEnd - origin; Vector3 firstBoneVector = secondBoneAbsoluteTransform.Translation - origin; Vector3 secondBoneVector = originToCurrentEnd - firstBoneVector; float firstBoneLength = firstBoneVector.Length(); float secondBoneLength = secondBoneVector.Length(); float originToDesiredEndLength = originToDesiredEnd.Length(); float originToCurrentEndLength = originToCurrentEnd.Length(); if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS) { VRageRender.MyRenderProxy.DebugDrawSphere(Vector3.Transform(desiredEnd, WorldMatrix), 0.01f, Color.Red, 1, false); VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToCurrentEnd, WorldMatrix), Color.Yellow, Color.Yellow, false); VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToDesiredEnd, WorldMatrix), Color.Red, Color.Red, false); VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + firstBoneVector, WorldMatrix), Color.Green, Color.Green, false); VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + firstBoneVector, WorldMatrix), Vector3.Transform(origin + firstBoneVector + secondBoneVector, WorldMatrix), Color.Blue, Color.Blue, false); } // only two cases, the desired position is reachable or not bool isDesiredEndReachable = firstBoneLength + secondBoneLength > originToDesiredEndLength; // alpha = angle between the first bone and originToDesiredEnd vector double finalAlpha = 0; // beta = the angle between the first and second bone double finalBeta = 0; if (isDesiredEndReachable) { // we find proper angles // cosine law c^2 = a^2 + b^2 - 2*a*b*cos(gamma) // gamma = acos ( - (c^2 - a^2 - b^2) / (2*a*b) ) // alpha = angle between the first bone and originToDesiredEnd vector double cosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToDesiredEndLength * originToDesiredEndLength) / (2 * firstBoneLength * originToDesiredEndLength); cosAlpha = MathHelper.Clamp(cosAlpha, -1, 1); finalAlpha = Math.Acos(cosAlpha); // beta = the angle between the first and second bone double cosBeta = -(originToDesiredEndLength * originToDesiredEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) / (2 * firstBoneLength * secondBoneLength); cosBeta = MathHelper.Clamp(cosBeta, -1, 1); finalBeta = Math.Acos(cosBeta); // now get it to the root bone axis no finalBeta = Math.PI - finalBeta; } // get the current angles double cCosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToCurrentEndLength * originToCurrentEndLength) / (2 * firstBoneLength * originToCurrentEndLength); cCosAlpha = MathHelper.Clamp(cCosAlpha, -1, 1); double currentAlpha = Math.Acos(cCosAlpha); double cCosBeta = -(originToCurrentEndLength * originToCurrentEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) / (2 * firstBoneLength * secondBoneLength); cCosBeta = MathHelper.Clamp(cCosBeta, -1, 1); double currentBeta = Math.Acos(cCosBeta); currentBeta = Math.PI - currentBeta; Vector3 currentPlaneNormal = Vector3.Cross(firstBoneVector, originToCurrentEnd); currentPlaneNormal.Normalize(); // we can now rotate the bones in current plane as if the desired end was on the currentEnd axis float alphaDif = (float)(finalAlpha - currentAlpha); float betaDif = (float)(finalBeta - currentBeta); Matrix firstBoneRotation = Matrix.CreateFromAxisAngle(-currentPlaneNormal, alphaDif); Matrix secondBoneRotation = Matrix.CreateFromAxisAngle(currentPlaneNormal, betaDif); // now get the angle between original and final position plane normal originToCurrentEnd.Normalize(); originToDesiredEnd.Normalize(); double dotProd = originToCurrentEnd.Dot(originToDesiredEnd); dotProd = MathHelper.Clamp(dotProd, -1, 1); double delta = Math.Acos(dotProd); Vector3 planeRotationAxis = Vector3.Cross(originToCurrentEnd, originToDesiredEnd); planeRotationAxis.Normalize(); // find the rotation matrices for bones in the original plane Matrix planeRotation = Matrix.CreateFromAxisAngle(planeRotationAxis, (float)delta); // compute the final rotations firstBoneRotation = planeRotation * firstBoneRotation; secondBoneRotation = secondBoneRotation * firstBoneRotation; // draw the final positions if debug enabled if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS) { Vector3 rotatedFirst = Vector3.Transform(firstBoneVector, firstBoneRotation); Vector3 rotatedSecond = Vector3.Transform(secondBoneVector, secondBoneRotation); VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + rotatedFirst, WorldMatrix), Color.Purple, Color.Purple, false); VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + rotatedFirst, WorldMatrix), Vector3.Transform(origin + rotatedFirst + rotatedSecond, WorldMatrix), Color.White, Color.White, false); } // Now we compute the final absolute transforms for the bones Matrix firstBoneFinalAbsoluteTransform = firstBoneAbsoluteTransform * firstBoneRotation; Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform; Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalAbsoluteTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform)); firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform); firstBone.ComputeAbsoluteTransform(); Matrix secondBoneFinalAbsoluteTransform = secondBoneAbsoluteTransform * secondBoneRotation; Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform; Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalAbsoluteTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform)); secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform); secondBone.ComputeAbsoluteTransform(); // solve the last bone if (finalBone != null && finalTransform.IsValid() && isDesiredEndReachable) { //MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation) // get the related transformation to original binding pose MatrixD localTransformRelated; if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform); else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform); //localTransformRelated = Matrix.Normalize(localTransformRelated); // from there get the rotation and translation finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation())); if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation; finalBone.ComputeAbsoluteTransform(); } return isDesiredEndReachable; }
public static bool SolveTwoJointsIkCCD(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true) { Vector3D rootPos, curEnd, targetVector, curVector, crossResult; double cosAngle, turnAngle; List<MyCharacterBone> bones = new List<MyCharacterBone>(); bones.Add(firstBone); bones.Add(secondBone); bones.Add(endBone); int tries = 0; int maxTries = 50; float stopDistance = 0.00001f; float gain = 0.6f; curEnd = Vector3.Zero; do { foreach (MyCharacterBone bone in bones.Reverse<MyCharacterBone>()) { // first recalculate current final transformation endBone.ComputeAbsoluteTransform(); // compute the position of the root Matrix currentMatrix = bone.AbsoluteTransform; rootPos = (Vector3D)currentMatrix.Translation; // this is this bone root position curEnd = (Vector3D)endBone.AbsoluteTransform.Translation; // this is our current end of the final bone // get the difference from desired and and current final position double distance = Vector3D.DistanceSquared(curEnd, desiredEnd); // see if i'm already close enough if (distance > stopDistance) { // create the vector to the current effector posm this is the difference vector curVector = curEnd - rootPos; // create the desired effector position vector targetVector = desiredEnd - rootPos; // normalize the vectors (expensive, requires a sqrt) curVector.Normalize(); targetVector.Normalize(); // the dot product gives me the cosine of the desired angle cosAngle = curVector.Dot(targetVector); // if the dot product returns 1.0, i don't need to rotate as it is 0 degrees if (cosAngle < 1.0) { // use the cross product to check which way to rotate crossResult = curVector.Cross(targetVector); crossResult.Normalize(); turnAngle = System.Math.Acos(cosAngle); // get the angle // get the matrix needed to rotate to the desired position Matrix rotation = Matrix.CreateFromAxisAngle((Vector3)crossResult, (float)turnAngle * gain); // get the absolute matrix rotation ie - rotation including all the bones before Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation; // compute just the local matrix for the bone - need to multiply with inversion ot its parent matrix and original bind transform Matrix parentMatrix = Matrix.Identity; if (bone.Parent != null) parentMatrix = bone.Parent.AbsoluteTransform; parentMatrix = Matrix.Normalize(parentMatrix); // may have different scale Matrix localTransform = Matrix.Multiply(absoluteTransform, Matrix.Invert(bone.BindTransform * parentMatrix)); // now change the current matrix rotation bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform); // and recompute the transformation bone.ComputeAbsoluteTransform(); } } } // quit if i am close enough or been running long enough } while (tries++ < maxTries && Vector3D.DistanceSquared(curEnd, desiredEnd) > stopDistance); // solve the last bone if (finalBone != null && finalTransform.IsValid()) { //MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation) // get the related transformation to original binding posefirstBoneAbsoluteTransform MatrixD localTransformRelated; if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform); else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform); //localTransformRelated = Matrix.Normalize(localTransformRelated); // from there get the rotation and translation finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation())); if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation; finalBone.ComputeAbsoluteTransform(); } return Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistance; }
private void ActivateRagdoll(Matrix worldMatrix) { if (MyFakes.ENABLE_RAGDOLL_DEBUG) { Debug.WriteLine("MyPhysicsBody.ActivateRagdoll"); MyLog.Default.WriteLine("MyPhysicsBody.ActivateRagdoll"); } if (Ragdoll == null) { Debug.Fail("Can not switch to Ragdoll mode, ragdoll is null!"); return; } if (HavokWorld == null) { Debug.Fail("Can not swtich to Ragdoll mode, HavokWorld is null!"); return; } if (IsRagdollModeActive) { Debug.Fail("Can not switch to ragdoll mode, ragdoll is still active!"); return; } //Matrix world = Entity.WorldMatrix; //world.Translation = WorldToCluster(world.Translation); Debug.Assert(worldMatrix.IsValid() && worldMatrix != Matrix.Zero, "Ragdoll world matrix is invalid!"); Ragdoll.SetWorldMatrix(worldMatrix); // Because after cluster's reorder, the bodies can collide! HavokWorld.AddRagdoll(Ragdoll); DisableRagdollBodiesCollisions(); if (MyFakes.ENABLE_RAGDOLL_DEBUG) { Debug.WriteLine("MyPhysicsBody.ActivateRagdoll - FINISHED"); MyLog.Default.WriteLine("MyPhysicsBody.ActivateRagdoll - FINISHED"); } }
public static bool SolveTwoJointsIkCCD(MyCharacterBone[] characterBones, int firstBoneIndex, int secondBoneIndex, int endBoneIndex, ref Matrix finalTransform, ref MatrixD worldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true) { if (finalBone == null) return false; Vector3 desiredEnd = finalTransform.Translation; //VRageRender.MyRenderProxy.DebugDrawSphere(Vector3D.Transform(desiredEnd, worldMatrix), 0.015f, Color.LightGoldenrodYellow, 1, false); Vector3 rootPos, curEnd; Vector3 curVector; double cosAngle, turnAngle; int tries = 0; int maxTries = 50; float stopDistanceSq = 0.005f * 0.005f; float gain = 0.65f; MyCharacterBone firstBone = characterBones[firstBoneIndex]; MyCharacterBone secondBone = characterBones[secondBoneIndex]; MyCharacterBone endBone = characterBones[endBoneIndex]; //unsafe { //int* boneIndices = stackalloc int[3]; int[] boneIndices = new int[3]; boneIndices[2] = firstBoneIndex; boneIndices[1] = secondBoneIndex; boneIndices[0] = endBoneIndex; curEnd = Vector3.Zero; for (int i = 0; i < 3; i++) { var bone = characterBones[boneIndices[i]]; Vector3 tempTranslation = bone.BindTransform.Translation; Quaternion tempRotation = Quaternion.CreateFromRotationMatrix(bone.BindTransform); bone.SetCompleteTransform(ref tempTranslation, ref tempRotation); bone.ComputeAbsoluteTransform(); } endBone.ComputeAbsoluteTransform(); curEnd = endBone.AbsoluteTransform.Translation; float initialDistSqInv = 1 / (float)Vector3D.DistanceSquared(curEnd, desiredEnd); do { for (int i = 0; i < 3; i++) { var bone = characterBones[boneIndices[i]]; // first recalculate current final transformation endBone.ComputeAbsoluteTransform(); // compute the position of the root Matrix currentMatrix = bone.AbsoluteTransform; rootPos = currentMatrix.Translation; // this is this bone root position Vector3 lastEnd = curEnd; curEnd = endBone.AbsoluteTransform.Translation; // this is our current end of the final bone // get the difference from desired and and current final position double distanceSq = Vector3D.DistanceSquared(curEnd, desiredEnd); //{ // Color c = Color.FromNonPremultiplied(new Vector4(4 * (float) (distanceSq), // 1 - 4 * (float) (distanceSq), 0, 1)); // VRageRender.MyRenderProxy.DebugDrawLine3D( // Vector3D.Transform(lastEnd, worldMatrix), // Vector3D.Transform(curEnd, worldMatrix), c, c, false); //} // see if i'm already close enough if (distanceSq > stopDistanceSq) { // create the vector to the current effector posm this is the difference vector curVector = curEnd - rootPos; // create the desired effector position vector var targetVector = desiredEnd - rootPos; // normalize the vectors (expensive, requires a sqrt) // MZ: we don't need to do that // curVector.Normalize(); // targetVector.Normalize(); double curVectorLenSq = curVector.LengthSquared(); double targetVectorLenSq = targetVector.LengthSquared(); // the dot product gives me the cosine of the desired angle // cosAngle = curVector.Dot(targetVector); double dotCurTarget = curVector.Dot(targetVector); // if the dot product returns 1.0, i don't need to rotate as it is 0 degrees // MZ: yes, but when does this happen to be exactly 1??? // if (cosAngle < 1.0) if (dotCurTarget < 0 || dotCurTarget * dotCurTarget < curVectorLenSq * targetVectorLenSq * (1 - MyMathConstants.EPSILON)) { // use the cross product to check which way to rotate //var rotationAxis = curVector.Cross(targetVector); //rotationAxis.Normalize(); //turnAngle = System.Math.Acos(cosAngle); // get the angle // get the matrix needed to rotate to the desired position //Matrix rotation = Matrix.CreateFromAxisAngle((Vector3) rotationAxis, // (float) turnAngle * gain); // get the absolute matrix rotation ie - rotation including all the bones before Matrix rotation; float weight = 1 / (initialDistSqInv * (float)distanceSq + 1); Vector3 weightedTarget = Vector3.Lerp(curVector, targetVector, weight); Matrix.CreateRotationFromTwoVectors(ref curVector, ref weightedTarget, out rotation); Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation; // MZ: faster // compute just the local matrix for the bone - need to multiply with inversion ot its parent matrix and original bind transform Matrix parentMatrix = Matrix.Identity; if (bone.Parent != null) parentMatrix = bone.Parent.AbsoluteTransform; parentMatrix = Matrix.Normalize(parentMatrix); // may have different scale Matrix localTransform = Matrix.Multiply(absoluteTransform, Matrix.Invert(bone.BindTransform * parentMatrix)); // now change the current matrix rotation bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform); // and recompute the transformation bone.ComputeAbsoluteTransform(); } } } // quit if i am close enough or been running long enough } while (tries++ < maxTries && Vector3D.DistanceSquared(curEnd, desiredEnd) > stopDistanceSq); } // solve the last bone if (finalTransform.IsValid()) { // get the related transformation to original binding posefirstBoneAbsoluteTransform MatrixD localTransformRelated; if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform); else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform); // localTransformRelated = Matrix.Normalize(localTransformRelated); // from there get the rotation and translation finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation())); if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation; finalBone.ComputeAbsoluteTransform(); } return true;//Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistanceSq; }
/// <summary> /// Called when [world position changed]. /// </summary> /// <param name="source">The source object that caused this event.</param> public override void OnWorldPositionChanged(object source) { if (IsInWorld == false) return; Debug.Assert(this != source, "Recursion!"); //Debug.Assert(Entity.Parent == null || RigidBody.IsFixedOrKeyframed); Vector3 velocity = Vector3.Zero; IMyEntity parentEntity = Entity.GetTopMostParent(); if (parentEntity.Physics != null) { velocity = parentEntity.Physics.LinearVelocity; //TODO:this should be optional, all our child bodies are kinematic and dependent on parent atm if (Entity != parentEntity) LinearVelocity = parentEntity.Physics.GetVelocityAtPoint(Entity.PositionComp.GetPosition()); } if(!IsWelded) MyPhysics.MoveObject(ClusterObjectID, parentEntity.WorldAABB, velocity); Matrix bodyMatrix; GetRigidBodyMatrix(out bodyMatrix); if (bodyMatrix.EqualsFast(ref m_bodyMatrix)) return; m_bodyMatrix = bodyMatrix; if (RigidBody != null) { RigidBody.SetWorldMatrix(m_bodyMatrix); } if (RigidBody2 != null) { RigidBody2.SetWorldMatrix(m_bodyMatrix); } if (CharacterProxy != null) { CharacterProxy.Position = m_bodyMatrix.Translation; CharacterProxy.Forward = m_bodyMatrix.Forward; CharacterProxy.Up = m_bodyMatrix.Up; CharacterProxy.Speed = 0; //if (CharacterProxy.ImmediateSetWorldTransform) { CharacterProxy.SetRigidBodyTransform(m_bodyMatrix); } } // TODO: This is disabled due to world synchronization, Ragdoll if set to some position from server doesn't simulate properly // Ragdoll updates it's position also in AfterUpdate on MyCharacter, so now this is not needed, but should be working. //if (Ragdoll != null && IsRagdollModeActive && m_ragdollDeadMode && !Sync.IsServer && MyFakes.ENABLE_RAGDOLL_CLIENT_SYNC) //{ // //Ragdoll.SetToKeyframed(); // //Ragdoll.SwitchToLayer(MyPhysics.CollisionLayers.RagdollCollisionLayer); // Ragdoll.SetWorldMatrix(rigidBodyMatrix,true); //} if (Ragdoll != null && IsRagdollModeActive && source is MyCockpit) { Debug.Assert(m_bodyMatrix.IsValid() && m_bodyMatrix != Matrix.Zero, "Ragdoll world matrix is invalid!"); Ragdoll.ResetToRigPose(); Ragdoll.SetWorldMatrix(m_bodyMatrix); Ragdoll.ResetVelocities(); } }