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