public static void GetCubeParts( MyCubeBlockDefinition block, Vector3I inputPosition, Matrix rotation, float gridSize, List<string> outModels, List<MatrixD> outLocalMatrices, List<Vector3> outLocalNormals, List<Vector2> outPatternOffsets) { // CH:TODO: Is rotation argument really needed as a Matrix? It should suffice for it to be MyBlockOrientation outModels.Clear(); outLocalMatrices.Clear(); outLocalNormals.Clear(); outPatternOffsets.Clear(); if (block.CubeDefinition == null) return; Base6Directions.Direction forward = Base6Directions.GetDirection(Vector3I.Round(rotation.Forward)); Base6Directions.Direction up = Base6Directions.GetDirection(Vector3I.Round(rotation.Up)); MyCubeGridDefinitions.GetTopologyUniqueOrientation(block.CubeDefinition.CubeTopology, new MyBlockOrientation(forward, up)).GetMatrix(out rotation); MyTileDefinition[] tiles = MyCubeGridDefinitions.GetCubeTiles(block); int count = tiles.Length; int start = 0; int avoidZeroMirrorOffset = 32768; float epsilon = 0.01f; for (int i = 0; i < count; i++) { var entry = tiles[start + i]; var localMatrix = (MatrixD)entry.LocalMatrix * rotation; var localNormal = Vector3.Transform(entry.Normal, rotation.GetOrientation()); var position = inputPosition; if(block.CubeDefinition.CubeTopology == MyCubeTopology.Slope2Base) { var addition = new Vector3I(Vector3.Sign(localNormal.MaxAbsComponent())); position += addition; } string modelPath = block.CubeDefinition.Model[i]; Vector2I patternSize = block.CubeDefinition.PatternSize[i]; int scale = (int)MyModels.GetModelOnlyData(modelPath).PatternScale; patternSize = new Vector2I(patternSize.X * scale, patternSize.Y * scale); const float sinConst = 10; int u = 0; int v = 0; float yAxis = Vector3.Dot(Vector3.UnitY, localNormal); float xAxis = Vector3.Dot(Vector3.UnitX, localNormal); float zAxis = Vector3.Dot(Vector3.UnitZ, localNormal); if (MyUtils.IsZero(Math.Abs(yAxis) - 1, epsilon)) { int patternRow = (position.X + avoidZeroMirrorOffset) / patternSize.Y; int offset = (MyMath.Mod(patternRow + (int)(patternRow * Math.Sin(patternRow * sinConst)), patternSize.X)); u = MyMath.Mod(position.Z + position.Y + offset + avoidZeroMirrorOffset, patternSize.X); v = MyMath.Mod(position.X + avoidZeroMirrorOffset, patternSize.Y); if (Math.Sign(yAxis) == 1) v = (patternSize.Y - 1) - v; } else if (MyUtils.IsZero(Math.Abs(xAxis) - 1, epsilon)) { int patternRow = (position.Z + avoidZeroMirrorOffset) / patternSize.Y; int offset = (MyMath.Mod(patternRow + (int)(patternRow * Math.Sin(patternRow * sinConst)), patternSize.X)); u = MyMath.Mod(position.X + position.Y + offset + avoidZeroMirrorOffset, patternSize.X); v = MyMath.Mod(position.Z + avoidZeroMirrorOffset, patternSize.Y); if (Math.Sign(xAxis) == 1) v = (patternSize.Y - 1) - v; } else if (MyUtils.IsZero(Math.Abs(zAxis) - 1, epsilon)) { int patternRow = (position.Y + avoidZeroMirrorOffset) / patternSize.Y; int offset = (MyMath.Mod(patternRow + (int)(patternRow * Math.Sin(patternRow * sinConst)), patternSize.X)); u = MyMath.Mod(position.X + offset + avoidZeroMirrorOffset, patternSize.X); v = MyMath.Mod(position.Y + avoidZeroMirrorOffset, patternSize.Y); if (Math.Sign(zAxis) == 1) u = (patternSize.X - 1) - u; } else if (MyUtils.IsZero(xAxis, epsilon)) { //slope in YZ u = MyMath.Mod(position.X + avoidZeroMirrorOffset, patternSize.X); v = MyMath.Mod(position.Z + avoidZeroMirrorOffset, patternSize.Y); if (Math.Sign(zAxis) == -1) { if (Math.Sign(yAxis) == 1) { //v = (patternSize.Y - 1) - v; //u = (patternSize.X - 1) - u; } else { // u = (patternSize.X - 1) - u; v = (patternSize.Y - 1) - v; } } else { if (Math.Sign(yAxis) == -1) { //u = (patternSize.X - 1) - u; v = (patternSize.Y - 1) - v; } else { //u = (patternSize.X - 1) - u; // v = (patternSize.Y - 1) - v; } } } else if (MyUtils.IsZero(zAxis, epsilon)) { //slope in XY u = MyMath.Mod(position.Z + avoidZeroMirrorOffset, patternSize.X); v = MyMath.Mod(position.Y + avoidZeroMirrorOffset, patternSize.Y); if (Math.Sign(xAxis) == 1) { if (Math.Sign(yAxis) == 1) { //u = (patternSize.X - 1) - u; //v = (patternSize.Y - 1) - v; } else { u = (patternSize.X - 1) - u; v = (patternSize.Y - 1) - v; } } else { if (Math.Sign(yAxis) == 1) { u = (patternSize.X - 1) - u; // v = (patternSize.Y - 1) - v; } else { // u = (patternSize.X - 1) - u; v = (patternSize.Y - 1) - v; } } } else if (MyUtils.IsZero(yAxis, epsilon)) { //slope in XZ u = MyMath.Mod(position.Y + avoidZeroMirrorOffset, patternSize.X); v = MyMath.Mod(position.Z + avoidZeroMirrorOffset, patternSize.Y); if (Math.Sign(zAxis) == -1) { if (Math.Sign(xAxis) == 1) { //u = (patternSize.X - 1) - u; v = (patternSize.Y - 1) - v; } else { u = (patternSize.X - 1) - u; v = (patternSize.Y - 1) - v; } } else { if (Math.Sign(xAxis) == 1) { u = (patternSize.X - 1) - u; //v = (patternSize.Y - 1) - v; } else { //u = (patternSize.X - 1) - u; // v = (patternSize.Y - 1) - v; } } } localMatrix.Translation = inputPosition * gridSize; if (entry.DontOffsetTexture) { u = 0; v = 0; } Vector2 uv = new Vector2(u, v); Vector2 patternOffset = uv / patternSize; outPatternOffsets.Add(patternOffset); outModels.Add(modelPath); outLocalMatrices.Add(localMatrix); outLocalNormals.Add(localNormal); } }
public bool IsCameraPositionOk(Matrix worldMatrix) { IMyCameraController cameraController = MySession.Static.CameraController; if (cameraController == null) return true; MyEntity topControlledEntity = ((MyEntity)cameraController).GetTopMostParent(); if (topControlledEntity.Closed) return false; var localAABBHr = topControlledEntity.PositionComp.LocalAABBHr; Vector3D center = Vector3D.Transform((Vector3D)localAABBHr.Center, worldMatrix); var safeOBB = new MyOrientedBoundingBoxD(center, localAABBHr.HalfExtents, Quaternion.CreateFromRotationMatrix(worldMatrix.GetOrientation())); //VRageRender.MyRenderProxy.DebugDrawOBB(safeOBB, Vector3.One, 1, false, false); //VRageRender.MyRenderProxy.DebugDrawAxis(topControlledEntity.WorldMatrix, 2, false); bool camPosIsOk = HandleIntersection(topControlledEntity, safeOBB, topControlledEntity is Sandbox.Game.Entities.Character.MyCharacter, true, m_target, m_targetOrientation.Forward); return camPosIsOk; }
public void SendRagdollTransforms(Matrix world, Matrix[] localBodiesTransforms) { if (ResponsibleForUpdate(Sync.Clients.LocalClient)) { Vector3 worldPosition = world.Translation; int transformsCount = localBodiesTransforms.Length; Quaternion worldOrientation = Quaternion.CreateFromRotationMatrix(world.GetOrientation()); Vector3[] transformsPositions = new Vector3[transformsCount]; Quaternion[] transformsOrientations = new Quaternion[transformsCount]; for (int i = 0; i < localBodiesTransforms.Length; ++i) { transformsPositions[i] = localBodiesTransforms[i].Translation; transformsOrientations[i] = Quaternion.CreateFromRotationMatrix(localBodiesTransforms[i].GetOrientation()); } MyMultiplayer.RaiseEvent(this, x => x.OnRagdollTransformsUpdate, transformsCount, transformsPositions, transformsOrientations, worldOrientation, worldPosition); } }
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; }
/// <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 void SendRagdollTransforms(Matrix world, Matrix[] localBodiesTransforms) { if (ResponsibleForUpdate(this)) { var msg = new RagdollTransformsMsg(); msg.CharacterEntityId = Entity.EntityId; msg.worldPosition = world.Translation; msg.TransformsCount = localBodiesTransforms.Count(); msg.worldOrientation = Quaternion.CreateFromRotationMatrix(world.GetOrientation()); msg.transformsPositions = new Vector3[msg.TransformsCount]; msg.transformsOrientations = new Quaternion[msg.TransformsCount]; for (int i = 0; i < localBodiesTransforms.Count(); ++i) { msg.transformsPositions[i] = localBodiesTransforms[i].Translation; msg.transformsOrientations[i] = Quaternion.CreateFromRotationMatrix(localBodiesTransforms[i].GetOrientation()); } Sync.Layer.SendMessageToAll(ref msg); } }
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> /// Calculates the force necessary to rotate the grid. /// </summary> /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param> /// <param name="Direction">The direction to face the localMatrix in.</param> /// <param name="angularVelocity">The local angular velocity of the controlling block.</param> private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity) { CheckGrid(); myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR); angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation()); //myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()"); float gyroForce = myGyro.TotalGyroForce(); const ulong UpdateFrequency = ShipController_Autopilot.UpdateFrequency; if (rotateForceRatio != Vector3.Zero) if (Globals.UpdateCount - updated_prevAngleVel == UpdateFrequency) // needs to be == because displacment is not divided by frequency { Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce); //myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()"); myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio); } else myLogger.debugLog("prevAngleVel is old: " + (Globals.UpdateCount - updated_prevAngleVel), "CalcRotate()", Logger.severity.DEBUG); localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1; Matrix inverted; Matrix.Invert(ref localMatrix, out inverted); localMatrix = localMatrix.GetOrientation(); inverted = inverted.GetOrientation(); //myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()"); //myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()"); //myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()"); //myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()"); Vector3 localDirect = Direction.ToLocal(); Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect); rotBlockDirect.Normalize(); float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation); Vector3 rotaRight = localMatrix.Right; Vector3 rotaUp = localMatrix.Up; Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight)); Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp)); Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up; if (UpDirect != null) { Vector3 upLocal = UpDirect.ToLocal(); Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock); float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll); Vector3 rotaBackward = localMatrix.Backward; Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward)); myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (-roll * NFR_backward), "CalcRotate()"); displacement += roll * NFR_backward; } NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length(); if (NavSet.Settings_Current.CollisionAvoidance) { myPathfinder.TestRotate(displacement); if (!myPathfinder.CanRotate) { Logger.debugNotify("Cannot Rotate", 50); myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()"); return; } } //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()"); if (myGyro.torqueAccelRatio == 0) { // do a test myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()"); rotateForceRatio = new Vector3(0, 1f, 0); return; } Vector3 targetVelocity = MaxAngleVelocity(displacement); // Adjust for moving target by measuring changes in displacement. Part of the change in displacement is attributable to ship rotation. const float dispToVel = (float)Globals.UpdatesPerSecond / (float)ShipController_Autopilot.UpdateFrequency; Vector3 addVelocity = angularVelocity - (prevAngleDisp - displacement) * dispToVel; if (addVelocity.LengthSquared() < 0.1f) { myLogger.debugLog("Adjust for moving, adding to target velocity: " + addVelocity, "CalcRotate()"); targetVelocity += addVelocity; } else myLogger.debugLog("Not adjusting for moving, assuming target changed: " + addVelocity, "CalcRotate()"); prevAngleDisp = displacement; Vector3 diffVel = targetVelocity - angularVelocity; rotateForceRatio = diffVel / (myGyro.torqueAccelRatio * gyroForce); myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", diffVel: " + diffVel, "CalcRotate()"); //myLogger.debugLog("diffVel: " + diffVel + ", torque: " + (myGyro.torqueAccelRatio * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()"); // dampeners for (int i = 0; i < 3; i++) { // if targetVelocity is close to 0, use dampeners float target = targetVelocity.GetDim(i); if (target > -0.01f && target < 0.01f) { myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()"); rotateForceRatio.SetDim(i, 0f); continue; } float velDim = angularVelocity.GetDim(i); if (velDim < 0.01f && velDim > -0.01f) continue; // where rotateForceRatio opposes angularVelocity, use dampeners float dim = rotateForceRatio.GetDim(i); if (Math.Sign(dim) * Math.Sign(angularVelocity.GetDim(i)) < 0) //{ // myLogger.debugLog("force ratio(" + dim + ") opposes velocity(" + angularVelocity.GetDim(i) + "), index: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()"); rotateForceRatio.SetDim(i, 0f); //} //else // myLogger.debugLog("force ratio is aligned with velocity: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()"); } }
/// <summary> /// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect. /// </summary> /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param> /// <param name="Direction">The direction to face the localMatrix in.</param> private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity) { m_logger.debugLog(Direction == null, "Direction == null", Logger.severity.ERROR); m_gyro.Update(); float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor); if (minimumMoment <= 0f) { // == 0f, not calculated yet. < 0f, we have math failure StopRotate(); m_logger.debugLog(minimumMoment < 0f, "minimumMoment < 0f", Logger.severity.FATAL); return; } localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1; Matrix inverted; Matrix.Invert(ref localMatrix, out inverted); localMatrix = localMatrix.GetOrientation(); inverted = inverted.GetOrientation(); Vector3 localDirect = Direction.ToLocalNormalized(); Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect); float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation); Vector3 rotaRight = localMatrix.Right; Vector3 rotaUp = localMatrix.Up; Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight)); Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp)); Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up; if (UpDirect != null) { Vector3 upLocal = UpDirect.ToLocalNormalized(); Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock); upRotBlock.Z = 0f; upRotBlock.Normalize(); float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f)); Vector3 rotaBackward = localMatrix.Backward; Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward)); //m_logger.debugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()"); displacement += roll * NFR_backward; } m_lastMoveAttempt = Globals.UpdateCount; Pathfinder.TestRotate(displacement); switch (Pathfinder.m_rotateState) { case Autopilot.Pathfinder.Pathfinder.PathState.Not_Running: m_logger.debugLog("Pathfinder not run yet: " + Pathfinder.m_rotateState); m_lastMove = Globals.UpdateCount; return; case Autopilot.Pathfinder.Pathfinder.PathState.No_Obstruction: break; default: m_logger.debugLog("Pathfinder not allowing rotation: " + Pathfinder.m_rotateState); return; } float distanceAngle = displacement.Length(); if (distanceAngle < m_bestAngle || float.IsNaN(m_navSet.Settings_Current.DistanceAngle)) { m_bestAngle = distanceAngle; m_lastMove = Globals.UpdateCount; } m_navSet.Settings_Task_NavWay.DistanceAngle = distanceAngle; //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()"); m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null); // adjustment to face a moving entity if (targetEntity != null) { Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - LinearVelocity; float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition()); //myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()"); float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down); float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right); float angl_pitch = (float)Math.Atan2(RLV_pitch, distance); float angl_yaw = (float)Math.Atan2(RLV_yaw, distance); m_logger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw)); m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw; } //m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()"); // angular velocity is reversed Vector3 angularVelocity = AngularVelocity.ToBlock(Block.CubeBlock);// ((DirectionWorld)(-Block.Physics.AngularVelocity)).ToBlock(Block.CubeBlock); m_rotateForceRatio = (m_rotateTargetVelocity + angularVelocity) / (minimumMoment * m_gyro.GyroForce); m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + (m_rotateTargetVelocity + angularVelocity)); m_logger.debugLog("minimumMoment: " + minimumMoment + ", force: " + m_gyro.GyroForce + ", rotateForceRatio: " + m_rotateForceRatio); // dampeners for (int index = 0; index < 3; index++) { // if targetVelocity is close to 0, use dampeners float target = m_rotateTargetVelocity.GetDim(index); if (target > -0.01f && target < 0.01f) { //m_logger.debugLog("target near 0 for " + i + ", " + target, "in_CalcRotate()"); m_rotateTargetVelocity.SetDim(index, 0f); m_rotateForceRatio.SetDim(index, 0f); continue; } } }
/// <summary> /// Calculates the force necessary to rotate the grid. /// </summary> /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param> /// <param name="Direction">The direction to face the localMatrix in.</param> /// <param name="angularVelocity">The local angular velocity of the controlling block.</param> private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity, IMyEntity targetEntity) { myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR); angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation()); //myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()"); float gyroForce = myGyro.TotalGyroForce(); float secondsSinceLast = (float)(DateTime.UtcNow - updated_prevAngleVel).TotalSeconds; updated_prevAngleVel = DateTime.UtcNow; if (rotateForceRatio != Vector3.Zero) { if (secondsSinceLast <= MaxUpdateSeconds) { Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce * secondsSinceLast); //myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()"); myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio); } else myLogger.debugLog("prevAngleVel is old: " + secondsSinceLast, "CalcRotate()", Logger.severity.DEBUG); } localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1; Matrix inverted; Matrix.Invert(ref localMatrix, out inverted); localMatrix = localMatrix.GetOrientation(); inverted = inverted.GetOrientation(); //myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()"); //myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()"); //myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()"); //myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()"); Vector3 localDirect = Direction.ToLocal(); Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect); rotBlockDirect.Normalize(); float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation); Vector3 rotaRight = localMatrix.Right; Vector3 rotaUp = localMatrix.Up; Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight)); Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp)); Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up; if (UpDirect != null) { Vector3 upLocal = UpDirect.ToLocal(); Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock); float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll); Vector3 rotaBackward = localMatrix.Backward; Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward)); myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (roll * NFR_backward), "CalcRotate()"); displacement += roll * NFR_backward; } NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length(); if (NavSet.Settings_Current.CollisionAvoidance) { myPathfinder.TestRotate(displacement); if (!myPathfinder.CanRotate) { // if cannot rotate and not calculating move, move away from obstruction if (myPathfinder.RotateObstruction != null && Globals.UpdateCount >= m_notCalcMove) { Vector3 position = Block.CubeBlock.GetPosition(); Vector3 away = position - myPathfinder.RotateObstruction.GetCentre(); away.Normalize(); myLogger.debugLog("Cannot rotate and not calculating move, creating GOLIS to move away from obstruction", "CalcRotate()", Logger.severity.INFO); new GOLIS(this, NavSet, position + away * (10f + NavSet.Settings_Current.DestinationRadius), true); } Logger.debugNotify("Cannot Rotate", 50); myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()"); return; } } //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()"); if (myGyro.torqueAccelRatio == 0) { // do a test myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()"); rotateForceRatio = new Vector3(0, 1f, 0); return; } Vector3 targetVelocity = MaxAngleVelocity(displacement, secondsSinceLast); if (targetEntity != null) { Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - Block.Physics.LinearVelocity; float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition()); //Vector3 tangentialVelocity = Vector3.Reject(relativeLinearVelocity, targetEntity.GetCentre() - Block.CubeBlock.GetPosition()); //Vector3 localTangVel = Vector3.Transform(tangentialVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation()); //myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "CalcRotate()"); float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Up); float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Left); float angl_pitch = (float)Math.Atan2(RLV_pitch, distance); float angl_yaw = (float)Math.Atan2(RLV_yaw, distance); myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch, "CalcRotate()"); targetVelocity += new Vector3(angl_pitch, angl_yaw, 0f); } Vector3 accel = (targetVelocity - angularVelocity) / secondsSinceLast; rotateForceRatio = accel / (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce); myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + accel, "CalcRotate()"); myLogger.debugLog("accel: " + accel + ", torque: " + (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()"); // dampeners for (int i = 0; i < 3; i++) { // if targetVelocity is close to 0, use dampeners float target = targetVelocity.GetDim(i); if (target > -0.01f && target < 0.01f) { //myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()"); rotateForceRatio.SetDim(i, 0f); continue; } } }