public void DebugDraw(ref Matrix drawMatrix, MyWingedEdgeMesh mesh)
{
Dictionary <Vector3I, int> .Enumerator enumerator = this.m_outerEdgePoints.EnumerateBins();
for (int i = 0; enumerator.MoveNext(); i++)
{
int binIndex = MyCestmirDebugInputComponent.BinIndex;
if ((binIndex == this.m_outerEdgePoints.InvalidIndex) || (i == binIndex))
{
BoundingBoxD xd;
BoundingBoxD xd2;
Vector3I key = enumerator.Current.Key;
int index = enumerator.Current.Value;
this.m_outerEdgePoints.GetLocalBinBB(ref key, out xd);
xd2.Min = Vector3D.Transform(xd.Min, drawMatrix);
xd2.Max = Vector3D.Transform(xd.Max, drawMatrix);
while (true)
{
if (index == this.m_outerEdgePoints.InvalidIndex)
{
MyRenderProxy.DebugDrawAABB(xd2, Color.PowderBlue, 1f, 1f, false, false, false);
break;
}
Vector3 point = this.m_outerEdgePoints.GetPoint(index);
MyWingedEdgeMesh.Edge edge = mesh.GetEdge(this.m_outerEdgePoints.GetData(index).EdgeIndex);
Vector3 vertexPosition = mesh.GetVertexPosition(edge.Vertex2);
Vector3D pointTo = Vector3D.Transform(point, drawMatrix);
MyRenderProxy.DebugDrawArrow3D(Vector3D.Transform((mesh.GetVertexPosition(edge.Vertex1) + vertexPosition) * 0.5f, drawMatrix), pointTo, Color.Yellow, new Color?(Color.Yellow), false, 0.1, null, 0.5f, false);
index = this.m_outerEdgePoints.GetNextBinIndex(index);
}
}
}
}
public void DebugDraw(MatrixD posAndOri)
{
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_AIMING == false)
{
return;
}
Vector3 pos2 = posAndOri.Translation;
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + posAndOri.Right, Color.Red, Color.Red, false, text: "X");
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + posAndOri.Up, Color.Green, Color.Green, false, text: "Y");
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + posAndOri.Forward, Color.Blue, Color.Blue, false, text: "-Z");
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + m_dbgDesiredForward, Color.Yellow, Color.Yellow, false, text: "Des.-Z");
Vector3D tip = pos2 + posAndOri.Forward;
MyRenderProxy.DebugDrawArrow3D(tip, tip + m_rotationHint.X * 10.0f * posAndOri.Right, Color.Salmon, Color.Salmon, false, text: "Rot.X");
MyRenderProxy.DebugDrawArrow3D(tip, tip - m_rotationHint.Y * 10.0f * posAndOri.Up, Color.LimeGreen, Color.LimeGreen, false, text: "Rot.Y");
var character = m_parent.BotEntity as MyCharacter;
if (character != null)
{
MyRenderProxy.DebugDrawSphere(character.AimedPoint, 0.2f, Color.Orange, 1, false);
}
}
public static void DebugDrawCoordinateSystem(Vector3?position, Vector3?forward, Vector3?side, Vector3?up, float scale = 1)
{
if (position.HasValue)
{
Vector3D p = position.Value;
if (forward.HasValue)
{
Vector3 f = forward.Value * scale;
MyRenderProxy.DebugDrawArrow3D(p, p + f, Color.Blue, Color.Red, false);
}
if (side.HasValue)
{
Vector3 s = side.Value * scale;
MyRenderProxy.DebugDrawArrow3D(p, p + s, Color.Blue, Color.Green, false);
}
if (up.HasValue)
{
Vector3 u = up.Value * scale;
MyRenderProxy.DebugDrawArrow3D(p, p + u, Color.Blue, Color.Blue, false);
}
}
}
public void DebugDraw()
{
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW == false)
{
return;
}
m_aiming.DebugDraw(m_aimingPositionAndOrientation);
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_STEERING)
{
foreach (var steering in m_steerings)
{
steering.DebugDraw();
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_NAVIGATION)
{
Vector3 pos = PositionAndOrientation.Translation;// + /*PositionAndOrientation.Up * 1.5f + */ForwardVector;
Vector3 rightVector = Vector3.Cross(m_forwardVector, UpVector);
if (Stuck)
{
MyRenderProxy.DebugDrawSphere(pos, 1.0f, Color.Red.ToVector3(), 1.0f, false);
}
//MyRenderProxy.DebugDrawLine3D(pos, pos + ForwardVector, Color.Blue, Color.Blue, false);
//var normalizedCorrection = Vector3D.Normalize(m_correction);
//var normalizedCorrectedDirXZ = normalizedCorrection + m_forwardVector;
//normalizedCorrectedDirXZ = Vector3D.Normalize(Vector3D.Reject(normalizedCorrectedDirXZ, Vector3D.Up));
//MyRenderProxy.DebugDrawLine3D(pos, pos + normalizedCorrectedDirXZ * 3, Color.Lime, Color.Lime, false);
MyRenderProxy.DebugDrawArrow3D(pos, pos + ForwardVector, Color.Blue, Color.Blue, false, text: "Nav. FW");
MyRenderProxy.DebugDrawArrow3D(pos + ForwardVector, pos + ForwardVector + m_correction, Color.LightBlue, Color.LightBlue, false, text: "Correction");
//MyRenderProxy.DebugDrawLine3D(pos + ForwardVector, pos + ForwardVector + m_correction, Color.Yellow, Color.Yellow, false);
//MyRenderProxy.DebugDrawSphere(pos + ForwardVector + m_correction, 0.05f, Color.Yellow.ToVector3(), 1.0f, true);
var character = this.BotEntity as MyCharacter;
if (character != null)
{
var viewMatrix = character.GetViewMatrix();
var worldMatrix = MatrixD.Invert(viewMatrix);
var headMatrix = character.GetHeadMatrix(true, true);
MyRenderProxy.DebugDrawLine3D(worldMatrix.Translation, Vector3D.Transform(Vector3D.Forward * 50, worldMatrix), Color.Yellow, Color.White, false);
MyRenderProxy.DebugDrawLine3D(headMatrix.Translation, Vector3D.Transform(Vector3D.Forward * 50, headMatrix), Color.Red, Color.Red, false);
if (character.CurrentWeapon != null)
{
var direction = character.CurrentWeapon.DirectionToTarget(character.AimedPoint);
var spos = (character.CurrentWeapon as MyEntity).WorldMatrix.Translation;
MyRenderProxy.DebugDrawSphere(character.AimedPoint, 1.0f, Color.Yellow, 1.0f, false);
MyRenderProxy.DebugDrawLine3D(spos, spos + direction * 20, Color.Purple, Color.Purple, false);
}
}
}
}
public static void DebugDrawVector3(Vector3?position, Vector3?vector, Color color, float scale = 0.01f)
{
if (position != null)
{
Vector3D pointFrom = position.Value;
if (vector != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (vector.Value * scale), color, new Color?(color), false, 0.1, null, 0.5f, false);
}
}
}
private void DrawNode(MyCubeGrid grid, MyGroups <MyCubeGrid, MyGridPhysicalHierarchyData> .Node node)
{
if (node.m_parents.Count <= 0)
{
MyRenderProxy.DebugDrawAxis(grid.PositionComp.WorldMatrix, 1f, false, false, false);
}
else
{
Color?colorTo = null;
MyRenderProxy.DebugDrawArrow3D(grid.PositionComp.GetPosition(), node.m_parents.FirstPair <long, MyGroups <MyCubeGrid, MyGridPhysicalHierarchyData> .Node>().Value.NodeData.PositionComp.GetPosition(), Color.Orange, colorTo, false, 0.1, null, 0.5f, false);
}
}
public static void DebugDrawVector3(Vector3?position, Vector3?vector, Color color, float scale = 0.01f)
{
if (position.HasValue)
{
Vector3D p = position.Value;
if (vector.HasValue)
{
Vector3 v = vector.Value * scale;
MyRenderProxy.DebugDrawArrow3D(p, p + v, color, color, false);
}
}
}
public static void DebugDrawAddForce(MyPhysicsBody physics, MyPhysicsForceType type, Vector3?force, Vector3D?position, Vector3?torque, bool persistent = false)
{
switch (type)
{
case MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE:
{
Vector3D pointFrom = position.Value + (physics.LinearVelocity * 0.01666667f);
if (force != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (force.Value * 0.1f), Color.Blue, new Color?(Color.Red), false, 0.1, null, 0.5f, persistent);
}
if (torque == null)
{
break;
}
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (torque.Value * 0.1f), Color.Blue, new Color?(Color.Purple), false, 0.1, null, 0.5f, persistent);
return;
}
case MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE:
if (physics.RigidBody != null)
{
Matrix rigidBodyMatrix = physics.RigidBody.GetRigidBodyMatrix();
Vector3D pointFrom = physics.CenterOfMassWorld + (physics.LinearVelocity * 0.01666667f);
if (force != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (Vector3.TransformNormal(force.Value, rigidBodyMatrix) * 0.1f), Color.Blue, new Color?(Color.Red), false, 0.1, null, 0.5f, persistent);
}
if (torque != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (Vector3.TransformNormal(torque.Value, rigidBodyMatrix) * 0.1f), Color.Blue, new Color?(Color.Purple), false, 0.1, null, 0.5f, persistent);
return;
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_FORCE:
if (position != null)
{
Vector3D pointFrom = position.Value + (physics.LinearVelocity * 0.01666667f);
if (force != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + ((force.Value * 0.01666667f) * 0.1f), Color.Blue, new Color?(Color.Red), false, 0.1, null, 0.5f, persistent);
}
}
break;
default:
return;
}
}
public static void DebugDrawCoordinateSystem(Vector3?position, Vector3?forward, Vector3?side, Vector3?up, float scale = 1f)
{
if (position != null)
{
Vector3D pointFrom = position.Value;
if (forward != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (forward.Value * scale), Color.Blue, new Color?(Color.Red), false, 0.1, null, 0.5f, false);
}
if (side != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (side.Value * scale), Color.Blue, new Color?(Color.Green), false, 0.1, null, 0.5f, false);
}
if (up != null)
{
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom + (up.Value * scale), Color.Blue, new Color?(Color.Blue), false, 0.1, null, 0.5f, false);
}
}
}
public void DebugDraw(MatrixD posAndOri)
{
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_AIMING)
{
Vector3 translation = (Vector3)posAndOri.Translation;
MyRenderProxy.DebugDrawArrow3D(translation, translation + posAndOri.Right, Color.Red, new Color?(Color.Red), false, 0.1, "X", 0.5f, false);
MyRenderProxy.DebugDrawArrow3D(translation, translation + posAndOri.Up, Color.Green, new Color?(Color.Green), false, 0.1, "Y", 0.5f, false);
MyRenderProxy.DebugDrawArrow3D(translation, translation + posAndOri.Forward, Color.Blue, new Color?(Color.Blue), false, 0.1, "-Z", 0.5f, false);
MyRenderProxy.DebugDrawArrow3D(translation, translation + this.m_dbgDesiredForward, Color.Yellow, new Color?(Color.Yellow), false, 0.1, "Des.-Z", 0.5f, false);
Vector3 pointFrom = translation + posAndOri.Forward;
MyRenderProxy.DebugDrawArrow3D(pointFrom, (translation + posAndOri.Forward) + ((this.m_rotationHint.X * 10f) * posAndOri.Right), Color.Salmon, new Color?(Color.Salmon), false, 0.1, "Rot.X", 0.5f, false);
MyRenderProxy.DebugDrawArrow3D(pointFrom, pointFrom - ((this.m_rotationHint.Y * 10f) * posAndOri.Up), Color.LimeGreen, new Color?(Color.LimeGreen), false, 0.1, "Rot.Y", 0.5f, false);
MyCharacter botEntity = this.m_parent.BotEntity as MyCharacter;
if (botEntity != null)
{
MyRenderProxy.DebugDrawSphere(botEntity.AimedPoint, 0.2f, Color.Orange, 1f, false, false, true, false);
}
}
}
public void DebugDraw()
{
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW)
{
this.m_aiming.DebugDraw(this.m_aimingPositionAndOrientation);
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_STEERING)
{
foreach (MySteeringBase local1 in this.m_steerings)
{
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_NAVIGATION)
{
Vector3 translation = (Vector3)this.PositionAndOrientation.Translation;
Vector3.Cross(this.m_forwardVector, this.UpVector);
if (this.Stuck)
{
MyRenderProxy.DebugDrawSphere(translation, 1f, Color.Red.ToVector3(), 1f, false, false, true, false);
}
MyRenderProxy.DebugDrawArrow3D(translation, translation + this.ForwardVector, Color.Blue, new Color?(Color.Blue), false, 0.1, "Nav. FW", 0.5f, false);
MyRenderProxy.DebugDrawArrow3D(translation + this.ForwardVector, (translation + this.ForwardVector) + this.m_correction, Color.LightBlue, new Color?(Color.LightBlue), false, 0.1, "Correction", 0.5f, false);
if (this.m_destinationSphere != null)
{
this.m_destinationSphere.DebugDraw();
}
MyCharacter botEntity = this.BotEntity as MyCharacter;
if (botEntity != null)
{
MatrixD matrix = MatrixD.Invert(botEntity.GetViewMatrix());
MatrixD xd3 = botEntity.GetHeadMatrix(true, true, false, false, false);
MyRenderProxy.DebugDrawLine3D(matrix.Translation, Vector3D.Transform(Vector3D.Forward * 50.0, matrix), Color.Yellow, Color.White, false, false);
MyRenderProxy.DebugDrawLine3D(xd3.Translation, Vector3D.Transform(Vector3D.Forward * 50.0, xd3), Color.Red, Color.Red, false, false);
if (botEntity.CurrentWeapon != null)
{
Vector3 vector2 = botEntity.CurrentWeapon.DirectionToTarget(botEntity.AimedPoint);
MyRenderProxy.DebugDrawSphere(botEntity.AimedPoint, 1f, Color.Yellow, 1f, false, false, true, false);
Vector3D pointFrom = (botEntity.CurrentWeapon as MyEntity).WorldMatrix.Translation;
MyRenderProxy.DebugDrawLine3D(pointFrom, (botEntity.CurrentWeapon as MyEntity).WorldMatrix.Translation + (vector2 * 20f), Color.Purple, Color.Purple, false, false);
}
}
}
}
}
public void DebugDraw(MatrixD posAndOri)
{
if (MyDebugDrawSettings.DEBUG_DRAW_BOT_AIMING == false)
{
return;
}
Vector3 pos2 = posAndOri.Translation;
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + posAndOri.Right, Color.Red, Color.Red, false, text: "X");
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + posAndOri.Up, Color.Green, Color.Green, false, text: "Y");
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + posAndOri.Forward, Color.Blue, Color.Blue, false, text: "-Z");
MyRenderProxy.DebugDrawArrow3D(pos2, pos2 + m_dbgDesiredForward, Color.Yellow, Color.Yellow, false, text: "Des.-Z");
Vector3D tip = pos2 + posAndOri.Forward;
MyRenderProxy.DebugDrawArrow3D(tip, tip + m_rotationHint.X * 10.0f * posAndOri.Right, Color.Salmon, Color.Salmon, false, text: "Rot.X");
MyRenderProxy.DebugDrawArrow3D(tip, tip - m_rotationHint.Y * 10.0f * posAndOri.Up, Color.LimeGreen, Color.LimeGreen, false, text: "Rot.Y");
}
// ------------------------------------------------------------------------------------
/// <summary>
/// Solve IK for chain of two bones + change rotation of end bone.
/// </summary>
/// <param name="characterBones">bone storage</param>
/// <param name="ikChain">description of bone chain</param>
/// <param name="finalPosition">desired position of end bone</param>
/// <param name="finalNormal">desired normal of end bone - would be projected on plane first bone-second bone-third bone</param>
/// <param name="fromBindPose">solve this starting from the bind pose</param>
/// <returns>true on success</returns>
public static bool SolveIkTwoBones(MyCharacterBone[] characterBones, MyAnimationIkChainExt ikChain, ref Vector3 finalPosition, ref Vector3 finalNormal, bool fromBindPose)
{
int boneIndex = ikChain.BoneIndex;
float finalMinRot = MathHelper.ToRadians(ikChain.MinEndPointRotation);
float finalMaxRot = MathHelper.ToRadians(ikChain.MaxEndPointRotation);
Vector3 lastPoleVector = ikChain.LastPoleVector;
int chainLength = ikChain.ChainLength;
bool alignBoneWithTerrain = ikChain.AlignBoneWithTerrain;
MyCharacterBone thirdBone = characterBones[boneIndex];
if (thirdBone == null)
{
return(false);
}
MyCharacterBone secondBone = thirdBone.Parent;
for (int i = 2; i < chainLength; i++)
{
secondBone = secondBone.Parent;
}
if (secondBone == null)
{
return(false);
}
MyCharacterBone firstBone = secondBone.Parent;
if (firstBone == null)
{
return(false);
}
if (fromBindPose)
{
firstBone.SetCompleteBindTransform();
secondBone.SetCompleteBindTransform();
thirdBone.SetCompleteBindTransform();
firstBone.ComputeAbsoluteTransform(true);
}
Matrix thirdBoneTransformBackup = thirdBone.AbsoluteTransform;
//Vector3 firstBoneTransformRightDir = firstBone.AbsoluteTransform.Right;
Vector3 firstBoneOrigin = firstBone.AbsoluteTransform.Translation;
Vector3 secondBoneOrigin = secondBone.AbsoluteTransform.Translation;
Vector3 thirdBoneOrigin = thirdBone.AbsoluteTransform.Translation;
// Vector3D finalPosition comes from parameter
Vector3 secondMinusFirst = secondBoneOrigin - firstBoneOrigin;
Vector3 finalMinusFirst = finalPosition - firstBoneOrigin;
//Vector3 finalMinusSecond = finalPosition - secondBoneOrigin;
Vector3 poleVectorNormalized;
Vector3 thirdMinusFirst = thirdBoneOrigin - firstBoneOrigin;
Vector3.Cross(ref secondMinusFirst, ref thirdMinusFirst, out poleVectorNormalized);
// project to 2D (only vectors)
poleVectorNormalized.Normalize();
poleVectorNormalized = Vector3.Normalize(Vector3.Lerp(poleVectorNormalized, lastPoleVector, m_poleVectorChangeSmoothness));
Vector3 planeDirY = Vector3.Normalize(finalMinusFirst); // finalMinusFirst? thirdMinusFirst?
Vector3 planeDirX = Vector3.Normalize(Vector3.Cross(planeDirY, poleVectorNormalized));
//Vector2 firstBoneOrigin2D = new Vector2(0, 0);
Vector2 secondBoneOrigin2D = new Vector2(planeDirX.Dot(ref secondMinusFirst), planeDirY.Dot(ref secondMinusFirst));
Vector2 thirdBoneOrigin2D = new Vector2(planeDirX.Dot(ref thirdMinusFirst), planeDirY.Dot(ref thirdMinusFirst));
Vector2 finalPosition2D = new Vector2(planeDirX.Dot(ref finalMinusFirst), planeDirY.Dot(ref finalMinusFirst));
Vector2 terrainNormal2D = new Vector2(planeDirX.Dot(ref finalNormal), planeDirY.Dot(ref finalNormal));
float firstBoneLength = (secondBoneOrigin2D /* - 0*/).Length();
float secondBoneLength = (thirdBoneOrigin2D - secondBoneOrigin2D).Length();
float finalDistance = finalPosition2D.Length();
if (firstBoneLength + secondBoneLength <= finalDistance)
{
finalPosition2D = (firstBoneLength + secondBoneLength) * finalPosition2D / finalDistance;
// too far
//return false;
}
// mid-joint -> wanted position in 2D
Vector2 newSecondBoneOrigin2D;
{
newSecondBoneOrigin2D.Y = (finalPosition2D.Y * finalPosition2D.Y - secondBoneLength * secondBoneLength + firstBoneLength * firstBoneLength) / (2.0f * finalPosition2D.Y);
float srqtArg = firstBoneLength * firstBoneLength - newSecondBoneOrigin2D.Y * newSecondBoneOrigin2D.Y;
newSecondBoneOrigin2D.X = (float)Math.Sqrt(srqtArg > 0 ? srqtArg : 0);
}
// project back
Vector3 newSecondBoneOrigin = firstBoneOrigin + planeDirX * newSecondBoneOrigin2D.X + planeDirY * newSecondBoneOrigin2D.Y;
Vector3 newSecondMinusFirst = newSecondBoneOrigin - firstBoneOrigin;
Vector3 newThirdMinusSecond = finalPosition - newSecondBoneOrigin;
Vector3 newTerrainNormal = planeDirX * terrainNormal2D.X + planeDirY * terrainNormal2D.Y;
newTerrainNormal.Normalize();
// set the rotations in the bones
// first bone ---------------------------------
Matrix firstBoneAbsoluteFinal = firstBone.AbsoluteTransform;
Quaternion rotFirstDelta = Quaternion.CreateFromTwoVectors(secondMinusFirst, newSecondMinusFirst);
firstBoneAbsoluteFinal.Right = Vector3.Transform(firstBoneAbsoluteFinal.Right, rotFirstDelta);
firstBoneAbsoluteFinal.Up = Vector3.Transform(firstBoneAbsoluteFinal.Up, rotFirstDelta);
firstBoneAbsoluteFinal.Forward = Vector3.Transform(firstBoneAbsoluteFinal.Forward, rotFirstDelta);
firstBone.SetCompleteTransformFromAbsoluteMatrix(ref firstBoneAbsoluteFinal, true);
firstBone.ComputeAbsoluteTransform();
// second bone ---------------------------------
Matrix secondBoneAbsoluteFinal = secondBone.AbsoluteTransform;
Quaternion rotSecondDelta = Quaternion.CreateFromTwoVectors(thirdBone.AbsoluteTransform.Translation - secondBone.AbsoluteTransform.Translation, newThirdMinusSecond);
secondBoneAbsoluteFinal.Right = Vector3.Transform(secondBoneAbsoluteFinal.Right, rotSecondDelta);
secondBoneAbsoluteFinal.Up = Vector3.Transform(secondBoneAbsoluteFinal.Up, rotSecondDelta);
secondBoneAbsoluteFinal.Forward = Vector3.Transform(secondBoneAbsoluteFinal.Forward, rotSecondDelta);
secondBone.SetCompleteTransformFromAbsoluteMatrix(ref secondBoneAbsoluteFinal, true);
secondBone.ComputeAbsoluteTransform();
//// third bone ----------------------------------
if (ikChain.EndBoneTransform.HasValue)
{
MatrixD localTransformRelated = ikChain.EndBoneTransform.Value * MatrixD.Invert((MatrixD)thirdBone.BindTransform * thirdBone.Parent.AbsoluteTransform);
thirdBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
thirdBone.Translation = (Vector3)localTransformRelated.Translation;
thirdBone.ComputeAbsoluteTransform();
}
else if (alignBoneWithTerrain)
{
Matrix footRotation;
Vector3 finalRotPoleVec;
Vector3.Cross(ref newTerrainNormal, ref Vector3.Up, out finalRotPoleVec);
float deltaAngle = MyUtils.GetAngleBetweenVectors(newTerrainNormal, Vector3.Up);
if (finalRotPoleVec.Dot(poleVectorNormalized) > 0)
{
deltaAngle = -deltaAngle;
}
deltaAngle = MathHelper.Clamp(deltaAngle, finalMinRot, finalMaxRot);
Matrix.CreateFromAxisAngle(ref poleVectorNormalized, deltaAngle, out footRotation);
ikChain.LastAligningRotationMatrix = Matrix.Lerp(ikChain.LastAligningRotationMatrix, footRotation, ikChain.AligningSmoothness);
Matrix thirdBoneAbsoluteFinal = thirdBoneTransformBackup.GetOrientation() * ikChain.LastAligningRotationMatrix;
thirdBoneAbsoluteFinal.Translation = thirdBone.AbsoluteTransform.Translation;
thirdBone.SetCompleteTransformFromAbsoluteMatrix(ref thirdBoneAbsoluteFinal, true);
thirdBone.ComputeAbsoluteTransform();
}
// Debugging of the solver.
if (m_showDebugDrawings)
{
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin, ref DebugTransform),
Color.Yellow, Color.Red, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(thirdBoneOrigin, ref DebugTransform),
Color.Yellow, Color.Red, false);
MyRenderProxy.DebugDrawSphere(Vector3D.Transform(finalPosition, ref DebugTransform), 0.05f, Color.Cyan, 1.0f, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin + poleVectorNormalized, ref DebugTransform),
Color.PaleGreen, Color.PaleGreen, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirX, ref DebugTransform),
Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirY, ref DebugTransform),
Color.White, Color.White, false);
MyRenderProxy.DebugDrawSphere(Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), 0.05f, Color.Green, 1.0f, false);
MyRenderProxy.DebugDrawAxis(firstBone.AbsoluteTransform * DebugTransform, 0.5f, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(finalPosition, ref DebugTransform), Vector3D.Transform(finalPosition + newTerrainNormal, ref DebugTransform),
Color.Black, Color.LightBlue, false);
MyRenderProxy.DebugDrawArrow3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform),
Color.Green, Color.White, false);
}
ikChain.LastPoleVector = poleVectorNormalized;
return(true);
}
public static void DebugDrawAddForce(MyPhysicsBody physics, MyPhysicsForceType type, Vector3?force, Vector3D?position, Vector3?torque)
{
Matrix transform;
const float scale = 0.1f;
switch (type)
{
case MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE:
{
if (physics.RigidBody != null)
{
transform = physics.RigidBody.GetRigidBodyMatrix();
Vector3D p = physics.CenterOfMassWorld + physics.LinearVelocity * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS; // ClusterToWorld(transform.Translation);//ClusterToWorld(transform.Translation);
if (force.HasValue)
{
Vector3 f = Vector3.TransformNormal(force.Value, transform) * scale;
MyRenderProxy.DebugDrawArrow3D(p, p + f, Color.Blue, Color.Red, false);
}
if (torque.HasValue)
{
Vector3 f = Vector3.TransformNormal(torque.Value, transform) * scale;
MyRenderProxy.DebugDrawArrow3D(p, p + f, Color.Blue, Color.Purple, false);
}
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE:
{
Vector3D p = position.Value + physics.LinearVelocity * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
if (force.HasValue)
{
MyRenderProxy.DebugDrawArrow3D(p, p + force.Value * scale, Color.Blue, Color.Red, false);
}
if (torque.HasValue)
{
MyRenderProxy.DebugDrawArrow3D(p, p + torque.Value * scale, Color.Blue, Color.Purple, false);
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_FORCE:
{
if (position.HasValue)
{
Vector3D p = position.Value + physics.LinearVelocity * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
if (force.HasValue)
{
MyRenderProxy.DebugDrawArrow3D(p, p + force.Value * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * scale, Color.Blue, Color.Red, false);
}
}
}
break;
default:
{
Debug.Fail("Unhandled enum!");
}
break;
}
}
public override void DebugDraw()
{
MatrixD worldMatrix;
Vector3D vectord4;
if (MyDebugDrawSettings.DEBUG_DRAW_GRID_AABB)
{
MatrixD worldMatrix = this.m_cubeGrid.PositionComp.WorldMatrix;
MyRenderProxy.DebugDrawOBB(new MyOrientedBoundingBoxD(this.m_cubeGrid.PositionComp.LocalAABB, worldMatrix), Color.Yellow, 0.2f, false, true, false);
MyRenderProxy.DebugDrawAxis(worldMatrix, 1f, false, false, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_FIXED_BLOCK_QUERIES)
{
foreach (MySlimBlock local1 in this.m_cubeGrid.GetBlocks())
{
Vector3D vectord;
Matrix matrix;
BoundingBox geometryLocalBox = local1.FatBlock.GetGeometryLocalBox();
Vector3 halfExtents = geometryLocalBox.Size / 2f;
local1.ComputeScaledCenter(out vectord);
vectord = Vector3D.Transform(vectord + geometryLocalBox.Center, this.m_cubeGrid.WorldMatrix);
local1.Orientation.GetMatrix(out matrix);
worldMatrix = this.m_cubeGrid.WorldMatrix;
Quaternion rotation = Quaternion.CreateFromRotationMatrix((MatrixD)(matrix * worldMatrix.GetOrientation()));
MyPhysics.GetPenetrationsBox(ref halfExtents, ref vectord, ref rotation, this.m_penetrations, 14);
bool flag = false;
using (List <HkBodyCollision> .Enumerator enumerator2 = this.m_penetrations.GetEnumerator())
{
while (enumerator2.MoveNext())
{
IMyEntity collisionEntity = enumerator2.Current.GetCollisionEntity();
if ((collisionEntity != null) && (collisionEntity is MyVoxelMap))
{
flag = true;
break;
}
}
}
this.m_penetrations.Clear();
MyRenderProxy.DebugDrawOBB(new MyOrientedBoundingBoxD(vectord, halfExtents, rotation), flag ? Color.Green : Color.Red, 0.1f, false, false, false);
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_GRID_NAMES || MyDebugDrawSettings.DEBUG_DRAW_GRID_CONTROL)
{
string text = "";
Color white = Color.White;
if (MyDebugDrawSettings.DEBUG_DRAW_GRID_NAMES)
{
text = text + this.m_cubeGrid.ToString() + " ";
}
if (MyDebugDrawSettings.DEBUG_DRAW_GRID_CONTROL)
{
MyPlayer controllingPlayer = Sync.Players.GetControllingPlayer(this.m_cubeGrid);
if (controllingPlayer != null)
{
text = text + "Controlled by: " + controllingPlayer.DisplayName;
white = Color.LightGreen;
}
}
MyRenderProxy.DebugDrawText3D(this.m_cubeGrid.PositionComp.WorldAABB.Center, text, white, 0.7f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER, -1, false);
}
MyRenderComponentCubeGrid render = this.m_cubeGrid.Render;
if (MyDebugDrawSettings.DEBUG_DRAW_BLOCK_GROUPS)
{
worldMatrix = this.m_cubeGrid.PositionComp.WorldMatrix;
Vector3D translation = worldMatrix.Translation;
foreach (MyBlockGroup group in this.m_cubeGrid.BlockGroups)
{
MyRenderProxy.DebugDrawText3D(translation, group.Name.ToString(), Color.Red, 1f, false, MyGuiDrawAlignEnum.HORISONTAL_LEFT_AND_VERTICAL_TOP, -1, false);
worldMatrix = this.m_cubeGrid.PositionComp.WorldMatrix;
translation += (worldMatrix.Right * group.Name.Length) * 0.10000000149011612;
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_GRID_DIRTY_BLOCKS)
{
foreach (KeyValuePair <Vector3I, MyTimeSpan> pair in this.m_dirtyBlocks)
{
Vector3 vector1;
if (this.m_cubeGrid.GetCubeBlock(pair.Key) == null)
{
vector1 = Color.Yellow.ToVector3();
}
else
{
vector1 = Color.Red.ToVector3();
}
Vector3 color = vector1;
MyRenderProxy.DebugDrawOBB((Matrix.CreateScale(this.m_cubeGrid.GridSize) * Matrix.CreateTranslation(pair.Key * this.m_cubeGrid.GridSize)) * this.m_cubeGrid.WorldMatrix, color, 0.15f, false, true, true, false);
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_DISPLACED_BONES)
{
Vector3D position = MySector.MainCamera.Position;
foreach (MySlimBlock block in this.m_cubeGrid.CubeBlocks)
{
MyCube cube;
if (this.m_cubeGrid.TryGetCube(block.Position, out cube))
{
int num = 0;
foreach (MyCubePart part in cube.Parts)
{
if ((part.Model.BoneMapping != null) && (num == MyPetaInputComponent.DEBUG_INDEX))
{
for (int i = 0; i < Math.Min(part.Model.BoneMapping.Length, 9); i++)
{
Matrix orientation = part.InstanceData.LocalMatrix.GetOrientation();
Vector3I vectori = Vector3I.Round(Vector3.Transform((Vector3)(((part.Model.BoneMapping[i] * 1f) - Vector3.One) * 1f), orientation));
Vector3I bonePos = Vector3I.Round(Vector3.Transform((Vector3)(((part.Model.BoneMapping[i] * 1f) - Vector3.One) * 1f), orientation) + Vector3.One);
Vector3 bone = this.m_cubeGrid.Skeleton.GetBone(block.Position, bonePos);
Vector3D vectord3 = Vector3D.TransformNormal(bone, this.m_cubeGrid.PositionComp.WorldMatrix);
Vector3D vectord6 = Vector3D.Transform((Vector3)(this.m_cubeGrid.GridSize * (block.Position + (vectori / 2f))), this.m_cubeGrid.PositionComp.WorldMatrix);
MyRenderProxy.DebugDrawSphere(vectord6, 0.025f, Color.Green, 0.5f, false, true, true, false);
MyRenderProxy.DebugDrawText3D(vectord6, i.ToString(), Color.Green, 0.5f, false, MyGuiDrawAlignEnum.HORISONTAL_LEFT_AND_VERTICAL_TOP, -1, false);
Color?colorTo = null;
MyRenderProxy.DebugDrawArrow3D(vectord6, vectord6 + vectord3, Color.Red, colorTo, false, 0.1, null, 0.5f, false);
}
}
num++;
}
}
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_STRUCTURAL_INTEGRITY && (this.m_cubeGrid.StructuralIntegrity != null))
{
this.m_cubeGrid.StructuralIntegrity.DebugDraw();
}
if (MyDebugDrawSettings.DEBUG_DRAW_CUBES)
{
foreach (MySlimBlock local2 in this.m_cubeGrid.CubeBlocks)
{
Matrix matrix4;
local2.GetLocalMatrix(out matrix4);
MyRenderProxy.DebugDrawAxis(matrix4 * this.m_cubeGrid.WorldMatrix, 1f, false, false, false);
MyCubeBlock fatBlock = local2.FatBlock;
if (fatBlock != null)
{
fatBlock.DebugDraw();
}
}
}
this.m_cubeGrid.GridSystems.DebugDraw();
bool flag1 = MyDebugDrawSettings.DEBUG_DRAW_GRID_TERMINAL_SYSTEMS;
if (MyDebugDrawSettings.DEBUG_DRAW_GRID_ORIGINS)
{
MyRenderProxy.DebugDrawAxis(this.m_cubeGrid.PositionComp.WorldMatrix, 1f, false, false, false);
}
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_MOUNT_POINTS_ALL)
{
foreach (MySlimBlock block3 in this.m_cubeGrid.GetBlocks())
{
vectord4 = this.m_cubeGrid.GridIntegerToWorld(block3.Position) - MySector.MainCamera.Position;
if (vectord4.LengthSquared() < 200.0)
{
this.DebugDrawMountPoints(block3);
}
}
}
if (MyDebugDrawSettings.DEBUG_DRAW_BLOCK_INTEGRITY && (MySector.MainCamera != null))
{
vectord4 = MySector.MainCamera.Position - this.m_cubeGrid.PositionComp.WorldVolume.Center;
if (vectord4.Length() < (16.0 + this.m_cubeGrid.PositionComp.WorldVolume.Radius))
{
using (HashSet <MySlimBlock> .Enumerator enumerator = this.m_cubeGrid.CubeBlocks.GetEnumerator())
{
MySlimBlock current;
float num4;
goto TR_0011;
TR_0002:
MyRenderProxy.DebugDrawText3D(this.m_cubeGrid.GridIntegerToWorld(current.Position), ((int)num4).ToString(), Color.White, (this.m_cubeGrid.GridSizeEnum == MyCubeSize.Large) ? 0.65f : 0.5f, false, MyGuiDrawAlignEnum.HORISONTAL_LEFT_AND_VERTICAL_TOP, -1, false);
TR_0011:
while (true)
{
if (!enumerator.MoveNext())
{
break;
}
current = enumerator.Current;
Vector3D vectord5 = this.m_cubeGrid.GridIntegerToWorld(current.Position);
if (this.m_cubeGrid.GridSizeEnum != MyCubeSize.Large)
{
if (MySector.MainCamera == null)
{
continue;
}
if ((MySector.MainCamera.Position - vectord5).LengthSquared() >= 9.0)
{
continue;
}
}
num4 = 0f;
if (current.FatBlock is MyCompoundCubeBlock)
{
foreach (MySlimBlock block5 in (current.FatBlock as MyCompoundCubeBlock).GetBlocks())
{
num4 += block5.Integrity * block5.BlockDefinition.MaxIntegrityRatio;
}
}
else
{
num4 = current.Integrity * current.BlockDefinition.MaxIntegrityRatio;
}
goto TR_0002;
}
}
}
}
base.DebugDraw();
}
public static void DebugDraw()
{
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_ENTITY_COMPONENTS && MySector.MainCamera != null)
{
double fontSize = 1.5;
double lineSize = fontSize * 0.045;
double hoffset = 0.5f;
Vector3D playerPos = MySector.MainCamera.Position;
Vector3D upVector = MySector.MainCamera.WorldMatrix.Up;
Vector3D rightVector = MySector.MainCamera.WorldMatrix.Right;
Vector3D fwVector = MySector.MainCamera.ForwardVector;
BoundingSphereD bSphere = new BoundingSphereD(playerPos, 5.0f);
var entities = MyEntities.GetEntitiesInSphere(ref bSphere);
Vector3D lastEntityPos = Vector3D.Zero;
Vector3D offset = Vector3D.Zero;
var mat = MySector.MainCamera.ViewProjectionMatrix;
var fullscreenRect = Sandbox.Graphics.MyGuiManager.GetSafeGuiRectangle();
float aspect = (float)fullscreenRect.Height / fullscreenRect.Width;
float scaleX = 600;
float scaleY = scaleX * aspect;
Vector3D worldAxisPos = playerPos + 1.0f * fwVector;
// Draw the world-space axis in the middle of the screen
/*MyRenderProxy.DebugDrawArrow3D(worldAxisPos, worldAxisPos + Vector3D.Right * 0.1f, Color.Red, Color.Red, false, text: "World X");
* MyRenderProxy.DebugDrawArrow3D(worldAxisPos, worldAxisPos + Vector3D.Up * 0.1f, Color.Green, Color.Green, false, text: "World Y");
* MyRenderProxy.DebugDrawArrow3D(worldAxisPos, worldAxisPos + Vector3D.Backward * 0.1f, Color.Blue, Color.Blue, false, text: "World Z");*/
Vector3D posView = Vector3D.Transform(worldAxisPos, mat);
Vector3D rtView = Vector3D.Transform(worldAxisPos + Vector3D.Right * 0.1f, mat);
Vector3D upView = Vector3D.Transform(worldAxisPos + Vector3D.Up * 0.1f, mat);
Vector3D bwView = Vector3D.Transform(worldAxisPos + Vector3D.Backward * 0.1f, mat);
var center2D = new Vector2((float)posView.X * scaleX, (float)posView.Y * -scaleY * aspect);
var right2D = new Vector2((float)rtView.X * scaleX, (float)rtView.Y * -scaleY * aspect) - center2D;
var up2D = new Vector2((float)upView.X * scaleX, (float)upView.Y * -scaleY * aspect) - center2D;
var backward2D = new Vector2((float)bwView.X * scaleX, (float)bwView.Y * -scaleY * aspect) - center2D;
var frameSize = 150.0f;
var frameBR = Sandbox.Graphics.MyGuiManager.GetScreenCoordinateFromNormalizedCoordinate(new Vector2(1.0f, 1.0f));
var frameBL = frameBR + new Vector2(-frameSize, 0.0f);
var frameTR = frameBR + new Vector2(0.0f, -frameSize);
var frameTL = frameBR + new Vector2(-frameSize, -frameSize);
var frameCenter = (frameBR + frameTL) * 0.5f;
// Draw frame around the world-space axis
/*MyRenderProxy.DebugDrawLine2D(frameTL, frameTR, Color.White, Color.White);
* MyRenderProxy.DebugDrawLine2D(frameTR, frameBR, Color.White, Color.White);
* MyRenderProxy.DebugDrawLine2D(frameBR, frameBL, Color.White, Color.White);
* MyRenderProxy.DebugDrawLine2D(frameBL, frameTL, Color.White, Color.White);*/
// Draw the world-space axis in the corner
MyRenderProxy.DebugDrawLine2D(frameCenter, frameCenter + right2D, Color.Red, Color.Red);
MyRenderProxy.DebugDrawLine2D(frameCenter, frameCenter + up2D, Color.Green, Color.Green);
MyRenderProxy.DebugDrawLine2D(frameCenter, frameCenter + backward2D, Color.Blue, Color.Blue);
MyRenderProxy.DebugDrawText2D(frameCenter + right2D, "World X", Color.Red, 0.5f, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
MyRenderProxy.DebugDrawText2D(frameCenter + up2D, "World Y", Color.Green, 0.5f, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
MyRenderProxy.DebugDrawText2D(frameCenter + backward2D, "World Z", Color.Blue, 0.5f, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
MyComponentsDebugInputComponent.DetectedEntities.Clear();
foreach (var entity in entities)
{
if (entity.PositionComp == null)
{
continue;
}
Vector3D originalPos = entity.PositionComp.GetPosition();
Vector3D pos2 = originalPos + upVector * 0.1f;
Vector3D pos = pos2 - rightVector * hoffset;
Vector3D viewVector = Vector3D.Normalize(originalPos - playerPos);
double dot = Vector3D.Dot(viewVector, fwVector);
if (dot < 0.9995)
{
Vector3D rightObject = entity.PositionComp.WorldMatrix.Right * 0.3f;
Vector3D upObject = entity.PositionComp.WorldMatrix.Up * 0.3f;
Vector3D backwardObject = entity.PositionComp.WorldMatrix.Backward * 0.3f;
MyRenderProxy.DebugDrawSphere(originalPos, 0.01f, Color.White, 1.0f, false);
MyRenderProxy.DebugDrawArrow3D(originalPos, originalPos + rightObject, Color.Red, Color.Red, false, text: "X");
MyRenderProxy.DebugDrawArrow3D(originalPos, originalPos + upObject, Color.Green, Color.Green, false, text: "Y");
MyRenderProxy.DebugDrawArrow3D(originalPos, originalPos + backwardObject, Color.Blue, Color.Blue, false, text: "Z");
continue;
}
if (Vector3D.Distance(originalPos, lastEntityPos) < 0.01)
{
offset += rightVector * 0.3f;
upVector = -upVector;
pos2 = originalPos + upVector * 0.1f;
pos = pos2 - rightVector * hoffset;
}
lastEntityPos = originalPos;
double dist = Vector3D.Distance(pos, playerPos);
double textSize = Math.Atan(fontSize / Math.Max(dist, 0.001));
float n = 0;
{
var enumerator = entity.Components.GetEnumerator();
MyComponentBase component = null;
while (enumerator.MoveNext())
{
component = enumerator.Current;
n += GetComponentLines(component);
}
n += 1;
n -= GetComponentLines(component); // The last component should not make the line longer
enumerator.Dispose();
}
Vector3D topPos = pos + (n + 0.5f) * upVector * lineSize;
Vector3D currentPos = pos + (n + 1) * upVector * lineSize + 0.01f * rightVector;
MyRenderProxy.DebugDrawLine3D(originalPos, pos2, Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(pos, pos2, Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(pos, topPos, Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(topPos, topPos + rightVector * 1.0f, Color.White, Color.White, false);
MyRenderProxy.DebugDrawText3D(currentPos, entity.GetType().ToString() + " - " + entity.DisplayName, Color.Orange, (float)textSize, false, MyGuiDrawAlignEnum.HORISONTAL_LEFT_AND_VERTICAL_CENTER);
MyComponentsDebugInputComponent.DetectedEntities.Add(entity);
foreach (var component in entity.Components)
{
currentPos = pos + n * upVector * lineSize;
DebugDrawComponent(component, currentPos, rightVector, upVector, lineSize, (float)textSize);
var entityComponent = component as MyEntityComponentBase;
string compType = entityComponent == null ? "" : entityComponent.ComponentTypeDebugString;
MyRenderProxy.DebugDrawText3D(currentPos - 0.02f * rightVector, compType, Color.Yellow, (float)textSize, false, MyGuiDrawAlignEnum.HORISONTAL_RIGHT_AND_VERTICAL_CENTER);
n -= GetComponentLines(component);
}
}
entities.Clear();
}
}
