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;
}
}
}
