public static NQuaternion GetDeltaQuaternionWithDirectionVectors(NVector3 a, NVector3 b)
{
var dot = NVector3.Dot(a, b);
if (dot < -0.999999)
{
var cross = NVector3.Cross(a, b);
if (cross.Length() < 0.000001)
{
cross = NVector3.Cross(NVector3.UnitY, a);
}
cross = NVector3.Normalize(cross);
return(NQuaternion.CreateFromAxisAngle(cross, Pi));
}
else if (dot > 0.999999)
{
return(new NQuaternion(0, 0, 0, 1));
}
else
{
var xyz = NVector3.Cross(a, b);
var w = (float)(Math.Sqrt(a.Length() * a.Length() + b.Length() * b.Length()) + dot);
return(new NQuaternion(xyz.X, xyz.Y, xyz.Z, w));
}
}
private Vector3 XXXSteerToEvadeAllDefenders()
{
// sum up weighted evasion
Vector3 evade = Vector3.Zero;
foreach (CtfEnemy e in Plugin.CtfEnemies)
{
Vector3 eOffset = e.Position - Position;
float eDistance = eOffset.Length();
// xxx maybe this should take into account e's heading? xxx
float timeEstimate = 0.5f * eDistance / e.Speed; //xxx
Vector3 eFuture = e.PredictFuturePosition(timeEstimate);
// annotation
annotation.CircleXZ(e.Radius, eFuture, Globals.EvadeColor.ToVector3().FromXna(), 20);
// steering to flee from eFuture (enemy's future position)
Vector3 flee = SteerForFlee(eFuture);
float eForwardDistance = Vector3.Dot(Forward, eOffset);
float behindThreshold = Radius * -2;
float distanceWeight = 4 / eDistance;
float forwardWeight = ((eForwardDistance > behindThreshold) ? 1.0f : 0.5f);
Vector3 adjustedFlee = flee * distanceWeight * forwardWeight;
evade += adjustedFlee;
}
return(evade);
}
/// <summary>
/// Creates a world matrix pointing from a position to a target with the given up vector.
/// </summary>
/// <param name="position">Position of the transform.</param>
/// <param name="forward">Forward direction of the transformation.</param>
/// <param name="upVector">Up vector which is crossed against the forward vector to compute the transform's basis.</param>
/// <param name="worldSystemMatrix4x4">World matrix.</param>
public static void CreateWorldRH(ref System.Numerics.Vector3 position, ref System.Numerics.Vector3 forward, ref System.Numerics.Vector3 upVector, out System.Numerics.Matrix4x4 worldSystemMatrix4x4)
{
System.Numerics.Vector3 z;
float length = forward.Length();
Vector3Ex.Divide(ref forward, -length, out z);
System.Numerics.Vector3 x;
Vector3Ex.Cross(ref upVector, ref z, out x);
x.Normalize();
System.Numerics.Vector3 y;
Vector3Ex.Cross(ref z, ref x, out y);
worldSystemMatrix4x4.M11 = x.X;
worldSystemMatrix4x4.M12 = x.Y;
worldSystemMatrix4x4.M13 = x.Z;
worldSystemMatrix4x4.M14 = 0f;
worldSystemMatrix4x4.M21 = y.X;
worldSystemMatrix4x4.M22 = y.Y;
worldSystemMatrix4x4.M23 = y.Z;
worldSystemMatrix4x4.M24 = 0f;
worldSystemMatrix4x4.M31 = z.X;
worldSystemMatrix4x4.M32 = z.Y;
worldSystemMatrix4x4.M33 = z.Z;
worldSystemMatrix4x4.M34 = 0f;
worldSystemMatrix4x4.M41 = position.X;
worldSystemMatrix4x4.M42 = position.Y;
worldSystemMatrix4x4.M43 = position.Z;
worldSystemMatrix4x4.M44 = 1f;
}
public Vector3 SteerToEvadeAllDefenders()
{
Vector3 evade = Vector3.Zero;
float goalDistance = Vector3.Distance(Globals.HomeBaseCenter, Position);
// sum up weighted evasion
foreach (CtfEnemy e in Plugin.CtfEnemies)
{
Vector3 eOffset = e.Position - Position;
float eDistance = eOffset.Length();
float eForwardDistance = Vector3.Dot(Forward, eOffset);
float behindThreshold = Radius * 2;
bool behind = eForwardDistance < behindThreshold;
if ((!behind) || (eDistance < 5))
{
if (eDistance < (goalDistance * 1.2)) //xxx
{
// const float timeEstimate = 0.5f * eDistance / e.speed;//xxx
float timeEstimate = 0.15f * eDistance / e.Speed; //xxx
Vector3 future = e.PredictFuturePosition(timeEstimate);
annotation.CircleXZ(e.Radius, future, Globals.EvadeColor.ToVector3().FromXna(), 20); // xxx
Vector3 offset = future - Position;
Vector3 lateral = Vector3Helpers.PerpendicularComponent(offset, Forward);
float d = lateral.Length();
float weight = -1000 / (d * d);
evade += (lateral / d) * weight;
}
}
}
return(evade);
}
/// <summary>
/// Creates a view matrix pointing looking in a direction with a given up vector.
/// </summary>
/// <param name="position">Position of the camera.</param>
/// <param name="forward">Forward direction of the camera.</param>
/// <param name="upVector">Up vector of the camera.</param>
/// <param name="viewSystem.Numerics.Matrix4x4">Look at matrix.</param>
public static void CreateViewRH(ref System.Numerics.Vector3 position, ref System.Numerics.Vector3 forward, ref System.Numerics.Vector3 upVector, out System.Numerics.Matrix4x4 viewSystemMatrix4x4)
{
System.Numerics.Vector3 z;
float length = forward.Length();
Vector3Ex.Divide(ref forward, -length, out z);
System.Numerics.Vector3 x;
Vector3Ex.Cross(ref upVector, ref z, out x);
x.Normalize();
System.Numerics.Vector3 y;
Vector3Ex.Cross(ref z, ref x, out y);
viewSystemMatrix4x4.M11 = x.X;
viewSystemMatrix4x4.M12 = y.X;
viewSystemMatrix4x4.M13 = z.X;
viewSystemMatrix4x4.M14 = 0f;
viewSystemMatrix4x4.M21 = x.Y;
viewSystemMatrix4x4.M22 = y.Y;
viewSystemMatrix4x4.M23 = z.Y;
viewSystemMatrix4x4.M24 = 0f;
viewSystemMatrix4x4.M31 = x.Z;
viewSystemMatrix4x4.M32 = y.Z;
viewSystemMatrix4x4.M33 = z.Z;
viewSystemMatrix4x4.M34 = 0f;
Vector3Ex.Dot(ref x, ref position, out viewSystemMatrix4x4.M41);
Vector3Ex.Dot(ref y, ref position, out viewSystemMatrix4x4.M42);
Vector3Ex.Dot(ref z, ref position, out viewSystemMatrix4x4.M43);
viewSystemMatrix4x4.M41 = -viewSystemMatrix4x4.M41;
viewSystemMatrix4x4.M42 = -viewSystemMatrix4x4.M42;
viewSystemMatrix4x4.M43 = -viewSystemMatrix4x4.M43;
viewSystemMatrix4x4.M44 = 1f;
}
public void Length()
{
Vec3 vec1 = new Vec3(3, 2, 1);
System.Numerics.Vector3 comp1 = new System.Numerics.Vector3(3, 2, 1);
float result = comp1.Length();
Assert.InRange(vec1.Length(), result - float.Epsilon, result + float.Epsilon);
}
/// <summary>
/// Computes a convex shape description for a TransformableShape.
/// </summary>
///<param name="vA">First local vertex in the triangle.</param>
///<param name="vB">Second local vertex in the triangle.</param>
///<param name="vC">Third local vertex in the triangle.</param>
///<param name="collisionMargin">Collision margin of the shape.</param>
/// <returns>Description required to define a convex shape.</returns>
public static ConvexShapeDescription ComputeDescription(System.Numerics.Vector3 vA, System.Numerics.Vector3 vB, System.Numerics.Vector3 vC, float collisionMargin)
{
ConvexShapeDescription description;
description.EntityShapeVolume.Volume = System.Numerics.Vector3.Cross(vB - vA, vC - vA).Length() * collisionMargin; // ratherapproximate.
description.EntityShapeVolume.VolumeDistribution = new Matrix3x3();
description.MinimumRadius = collisionMargin;
description.MaximumRadius = collisionMargin + Math.Max(vA.Length(), Math.Max(vB.Length(), vC.Length()));
description.CollisionMargin = collisionMargin;
return(description);
}
public static float Angle(this Vector3d A, Vector3d B)
{
float vDot = Vector3d.Dot(A, B) / (A.Length() * B.Length());
if (vDot < -1.0f)
{
vDot = -1.0f;
}
if (vDot > 1.0f)
{
vDot = 1.0f;
}
return((float)Math.Acos(vDot));
}
//----------------------------------CONSTRUCTORS---------------------------------//
/**
* Constructor for a line. The line created is the intersection between two planes
*
* @param face1 face representing one of the planes
* @param face2 face representing one of the planes
*/
public Line(Face face1, Face face2)
{
Vector3d normalFace1 = face1.getNormal();
Vector3d normalFace2 = face2.getNormal();
//direction: cross product of the faces normals
//direction = new Vector3d();
//direction.cross(normalFace1, normalFace2);
direction = Vector3d.Cross(normalFace1, normalFace2);
//if direction lenght is not zero (the planes aren't parallel )...
if (!(direction.Length() < TOL))
{
//getting a line point, zero is set to a coordinate whose direction
//component isn't zero (line intersecting its origin plan)
point = new Point3d();
float d1 = -(normalFace1.X * face1.v1.X + normalFace1.Y * face1.v1.Y + normalFace1.Z * face1.v1.Z);
float d2 = -(normalFace2.X * face2.v1.X + normalFace2.Y * face2.v1.Y + normalFace2.Z * face2.v1.Z);
if (Math.Abs(direction.X) > TOL)
{
point.X = 0;
point.Y = (d2 * normalFace1.Z - d1 * normalFace2.Z) / direction.X;
point.Z = (d1 * normalFace2.Y - d2 * normalFace1.Y) / direction.X;
}
else if (Math.Abs(direction.Y) > TOL)
{
point.X = (d1 * normalFace2.Z - d2 * normalFace1.Z) / direction.Y;
point.Y = 0;
point.Z = (d2 * normalFace1.X - d1 * normalFace2.X) / direction.Y;
}
else
{
point.X = (d2 * normalFace1.Y - d1 * normalFace2.Y) / direction.Z;
point.Y = (d1 * normalFace2.X - d2 * normalFace1.X) / direction.Z;
point.Z = 0;
}
}
direction = Vector3d.Normalize(direction);
}
// is there a clear path to the goal?
private bool IsPathToGoalClear()
{
float sideThreshold = Radius * 8.0f;
float behindThreshold = Radius * 2.0f;
Vector3 goalOffset = Globals.HomeBaseCenter - Position;
float goalDistance = goalOffset.Length();
Vector3 goalDirection = goalOffset / goalDistance;
bool goalIsAside = this.IsAside(Globals.HomeBaseCenter, 0.5f);
// for annotation: loop over all and save result, instead of early return
bool xxxReturn = true;
// loop over enemies
foreach (CtfEnemy e in Plugin.CtfEnemies)
{
float eDistance = Vector3.Distance(Position, e.Position);
float timeEstimate = 0.3f * eDistance / e.Speed; //xxx
Vector3 eFuture = e.PredictFuturePosition(timeEstimate);
Vector3 eOffset = eFuture - Position;
float alongCorridor = Vector3.Dot(goalDirection, eOffset);
bool inCorridor = ((alongCorridor > -behindThreshold) && (alongCorridor < goalDistance));
float eForwardDistance = Vector3.Dot(Forward, eOffset);
// xxx temp move this up before the conditionals
annotation.CircleXZ(e.Radius, eFuture, Globals.ClearPathColor.ToVector3().FromXna(), 20); //xxx
// consider as potential blocker if within the corridor
if (inCorridor)
{
Vector3 perp = eOffset - (goalDirection * alongCorridor);
float acrossCorridor = perp.Length();
if (acrossCorridor < sideThreshold)
{
// not a blocker if behind us and we are perp to corridor
float eFront = eForwardDistance + e.Radius;
//annotation.annotationLine (position, forward*eFront, gGreen); // xxx
//annotation.annotationLine (e.position, forward*eFront, gGreen); // xxx
// xxx
// std::ostringstream message;
// message << "eFront = " << std::setprecision(2)
// << std::setiosflags(std::ios::fixed) << eFront << std::ends;
// draw2dTextAt3dLocation (*message.str(), eFuture, gWhite);
bool eIsBehind = eFront < -behindThreshold;
bool eIsWayBehind = eFront < (-2 * behindThreshold);
bool safeToTurnTowardsGoal = ((eIsBehind && goalIsAside) || eIsWayBehind);
if (!safeToTurnTowardsGoal)
{
// this enemy blocks the path to the goal, so return false
annotation.Line(Position, e.Position, Globals.ClearPathColor.ToVector3().FromXna());
// return false;
xxxReturn = false;
}
}
}
}
// no enemies found along path, return true to indicate path is clear
// clearPathAnnotation (sideThreshold, behindThreshold, goalDirection);
// return true;
//if (xxxReturn)
ClearPathAnnotation(sideThreshold, behindThreshold, goalDirection);
return(xxxReturn);
}
protected override void Update(GameTime gameTime)
{
KeyboardState state = Keyboard.GetState();
MouseState mState = Mouse.GetState();
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == Microsoft.Xna.Framework.Input.ButtonState.Pressed || Keyboard.GetState().IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Escape))
{
Application.Exit();
}
float delta = (float)gameTime.ElapsedGameTime.TotalSeconds;
//Marine_Yes00.mp3
if (mState.LeftButton == Microsoft.Xna.Framework.Input.ButtonState.Pressed && IsActive)
{
float mdx = mState.X - prevMState.X;
float mdy = mState.Y - prevMState.Y;
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
System.Numerics.Vector3 p2 = Program.RotatePoint(p, 0, 0, -mdx * 0.01f);
cameraX = p2.X;
cameraY = p2.Y;
cameraZ = p2.Z;
}
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
float nX = cameraY;
float nY = -cameraX;
System.Numerics.Vector3 p2 = Program.RotateLine(p, new System.Numerics.Vector3(0, 0, 0),
new System.Numerics.Vector3(nX, nY, 0), mdy * 0.01f);
cameraX = p2.X;
cameraY = p2.Y;
cameraZ = p2.Z;
}
}
if (mState.MiddleButton == Microsoft.Xna.Framework.Input.ButtonState.Pressed && IsActive)
{
float mdx = mState.X - prevMState.X;
float mdy = mState.Y - prevMState.Y;
// offsetZ += mdy * 3 * delta;
Vector3D upV = new Vector3D(0, 0, 1);
Vector3D forwardV = new Vector3D(cameraX, cameraY, cameraZ);
Vector3D rightV = Vector3D.crossPorduct(upV, forwardV).normalize();
Vector3D camUpV = Vector3D.crossPorduct(forwardV, rightV).normalize();
Vector3D offsetV = new Vector3D(offsetX, offsetY, offsetZ);
offsetV = offsetV - new Vector3D(rightV.X * mdx * 0.01f, rightV.Y * mdx * 0.01f, rightV.Z * mdx * 0.01f);
offsetV = offsetV + new Vector3D(camUpV.X * mdy * 0.01f, camUpV.Y * mdy * 0.01f, camUpV.Z * mdy * 0.01f);
offsetX = offsetV.X;
offsetY = offsetV.Y;
offsetZ = offsetV.Z;
//offsetX -= mdx* 1 * delta * rightV.X;
//offsetY -= mdx * 1 * delta * rightV.Y;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.F1))
{
renderMode = RenderMode.Line;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.F2))
{
renderMode = RenderMode.Triangle;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.F3))
{
renderMode = RenderMode.Both;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.B) && !prevState.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.B))
{
Program.boneDisplay = !Program.boneDisplay;
Program.updateVertices();
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.M) && !prevState.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.M))
{
Program.dummyDisplay = !Program.dummyDisplay;
Program.updateVertices();
}
//1.73 Added focus detect
if (mState.RightButton == Microsoft.Xna.Framework.Input.ButtonState.Pressed && this.IsActive && false)
{
Ray r = GetMouseRay(new Vector2(mState.Position.X, mState.Position.Y), GraphicsDevice.Viewport, effect);
r.Position = new Vector3(r.Position.X, r.Position.Z, r.Position.Y);
r.Direction = new Vector3(r.Direction.X, r.Direction.Z, r.Direction.Y);
// Vector3D x1 = new Vector3D(cameraX + offsetX, cameraY + offsetY, cameraZ + offsetZ);
//Vector3D x2 = new Vector3D(centerX + offsetX, centerY + offsetY, centerZ + offsetZ);
Vector3D x1 = new Vector3D(r.Position);
Vector3D x2 = new Vector3D(r.Position + r.Direction);
//Program.useCheckingPoint = true;
// Program.checkingPoint = new System.Numerics.Vector3(x2.X,x2.Z,x2.Y);
// Program.updateVertices();
Vector3D miniPoint = new Vector3D();
float ptDistance = float.MaxValue;
SoulsFormats.FLVER.Vertex targetV = null;
foreach (SoulsFormats.FLVER.Vertex v in Program.vertices)
{
if (v.Positions[0] == null)
{
continue;
}
float dis = Vector3D.calculateDistanceFromLine(new Vector3D(v.Positions[0]), x1, x2);
if (ptDistance > dis)
{
miniPoint = new Vector3D(v.Positions[0]);
ptDistance = dis;
targetV = v;
}
}
if (Program.setVertexPos)
{
targetV.Positions[0] = new Vector3D(Program.setVertexX, Program.setVertexY, Program.setVertexZ).toNumV3();
}
Program.useCheckingPoint = true;
Program.checkingPoint = new System.Numerics.Vector3(miniPoint.X, miniPoint.Y, miniPoint.Z);
if (targetV.Normals != null && targetV.Normals.Count > 0)
{
Program.checkingPointNormal = new System.Numerics.Vector3(targetV.Normals[0].X, targetV.Normals[0].Y, targetV.Normals[0].Z);
}
else
{
Program.checkingPointNormal = new System.Numerics.Vector3(0, 0, 0);
}
Program.updateVertices();
if (targetV != null)
{
string text = Program.FormatOutput(new System.Web.Script.Serialization.JavaScriptSerializer().Serialize(targetV));
int l = text.Length / 2;
System.Windows.Forms.MessageBox.Show(text.Substring(0, l), "Vertex info1:");
System.Windows.Forms.MessageBox.Show(text.Substring(l, text.Length - l), "Vertex info2:");
}
}
if (mState.ScrollWheelValue - prevMState.ScrollWheelValue > 0)
{
//mouseY -= (50 * delta);
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
cameraX = p.X - 0.5f * (float)(p.X / p.Length());
cameraY = p.Y - 0.5f * (float)(p.Y / p.Length());
cameraZ = p.Z - 0.5f * (float)(p.Z / p.Length());
}
if (mState.ScrollWheelValue - prevMState.ScrollWheelValue < 0)
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
cameraX = p.X + 0.5f * (float)(p.X / p.Length());
cameraY = p.Y + 0.5f * (float)(p.Y / p.Length());
cameraZ = p.Z + 0.5f * (float)(p.Z / p.Length());
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Right))
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
System.Numerics.Vector3 p2 = Program.RotatePoint(p, 0, 0, 5 * delta);
cameraX = p2.X;
cameraY = p2.Y;
cameraZ = p2.Z;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Left))
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
System.Numerics.Vector3 p2 = Program.RotatePoint(p, 0, 0, -5 * delta);
cameraX = p2.X;
cameraY = p2.Y;
cameraZ = p2.Z;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Up))
{
//mouseY -= (50 * delta);
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
System.Numerics.Vector3 p2 = Program.RotateLine(p, new System.Numerics.Vector3(0, 0, 0),
new System.Numerics.Vector3(cameraY, -cameraX, 0), 3 * delta);
cameraX = p2.X;
cameraY = p2.Y;
cameraZ = p2.Z;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.Down))
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
System.Numerics.Vector3 p2 = Program.RotateLine(p, new System.Numerics.Vector3(0, 0, 0),
new System.Numerics.Vector3(cameraY, -cameraX, 0), -3 * delta);
cameraX = p2.X;
cameraY = p2.Y;
cameraZ = p2.Z;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.OemComma))
{
//mouseY -= (50 * delta);
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
cameraX = p.X - 3 * delta * (float)(p.X / p.Length());
cameraY = p.Y - 3 * delta * (float)(p.Y / p.Length());
cameraZ = p.Z - 3 * delta * (float)(p.Z / p.Length());
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.OemPeriod))
{
System.Numerics.Vector3 p = new System.Numerics.Vector3(cameraX, cameraY, cameraZ);
cameraX = p.X + 3 * delta * (float)(p.X / p.Length());
cameraY = p.Y + 3 * delta * (float)(p.Y / p.Length());
cameraZ = p.Z + 3 * delta * (float)(p.Z / p.Length());
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.NumPad8))
{
offsetZ += 3 * delta;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.NumPad2))
{
offsetZ -= 3 * delta;;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.NumPad4))
{
Vector3D upV = new Vector3D(0, 0, 1);
Vector3D forwardV = new Vector3D(cameraX, cameraY, cameraZ);
Vector3D rightV = Vector3D.crossPorduct(upV, forwardV).normalize();
offsetX -= 3 * delta * rightV.X;
offsetY -= 3 * delta * rightV.Y;
//offsetZ -= 3 * delta; ;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.NumPad6))
{
Vector3D upV = new Vector3D(0, 0, 1);
Vector3D forwardV = new Vector3D(cameraX, cameraY, cameraZ);
Vector3D rightV = Vector3D.crossPorduct(upV, forwardV).normalize();
offsetX += 3 * delta * rightV.X;
offsetY += 3 * delta * rightV.Y;
//offsetZ -= 3 * delta; ;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.NumPad5))
{
Vector3D forwardV = new Vector3D(cameraX, cameraY, cameraZ).normalize();
offsetX -= 3 * delta * forwardV.X;
offsetY -= 3 * delta * forwardV.Y;
offsetZ -= 3 * delta * forwardV.Z;
//offsetZ -= 3 * delta; ;
}
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.NumPad0))
{
Vector3D forwardV = new Vector3D(cameraX, cameraY, cameraZ).normalize();
offsetX += 3 * delta * forwardV.X;
offsetY += 3 * delta * forwardV.Y;
offsetZ += 3 * delta * forwardV.Z;
//offsetZ -= 3 * delta; ;
}
//new Vector3(cameraX + offsetX, cameraY + offsetY, cameraZ + offsetZ)
/* if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.D1)) { Program.rotOrder = RotationOrder.XYZ; Program.updateVertices(); }
* if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.D2)) { Program.rotOrder = RotationOrder.XZY; Program.updateVertices(); }
* if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.D3)) { Program.rotOrder = RotationOrder.YXZ; Program.updateVertices(); }
* if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.D4)) { Program.rotOrder = RotationOrder.YZX; Program.updateVertices(); }
* if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.D5)) { Program.rotOrder = RotationOrder.ZXY; Program.updateVertices(); }
* if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.D6)) { Program.rotOrder = RotationOrder.ZYX; Program.updateVertices(); }*/
if (state.IsKeyDown(Microsoft.Xna.Framework.Input.Keys.F))
{
Program.updateVertices();
}
//mouseX = Mouse.GetState().Position.X;
//mouseY = Mouse.GetState().Position.Y;
// TODO: Add your update logic here
prevState = state;
prevMState = mState;
base.Update(gameTime);
}
public void Kick(Vector3 dir)
{
Speed = (dir.Length());
RegenerateOrthonormalBasis(dir);
}
public void Kick(Vector3 dir)
{
Speed = (dir.Length());
RegenerateOrthonormalBasis(dir);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform point into world space.
Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r);
Vector3Ex.Add(ref r, ref entity.position, out worldPoint);
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism)
{
Vector3Ex.Subtract(ref settings.servo.goal, ref worldPoint, out error);
float separationDistance = error.Length();
if (separationDistance > Toolbox.BigEpsilon)
{
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
//The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity.
//The constant correction velocity could overshoot the destination, so clamp it.
float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) +
separationDistance * errorReduction;
Vector3Ex.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity);
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > settings.servo.maxCorrectiveVelocitySquared)
{
float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
else
{
//Wouldn't want to use a bias from an earlier frame.
biasVelocity = new System.Numerics.Vector3();
}
}
else
{
usedSoftness = settings.velocityMotor.softness * updateRate;
biasVelocity = settings.velocityMotor.goalVelocity;
error = System.Numerics.Vector3.Zero;
}
//Compute the maximum force that can be applied this frame.
ComputeMaxForces(settings.maximumForce, dt);
//COMPUTE EFFECTIVE MASS MATRIX
//Transforms a change in velocity to a change in momentum when multiplied.
Matrix3x3 linearComponent;
Matrix3x3.CreateScale(entity.inverseMass, out linearComponent);
Matrix3x3 rACrossProduct;
Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
}
private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//Sample the local directions from the matrix, implicitly transposed.
var rightDirection = new System.Numerics.Vector3(o.M11, o.M21, o.M31);
var upDirection = new System.Numerics.Vector3(o.M12, o.M22, o.M32);
var backDirection = new System.Numerics.Vector3(o.M13, o.M23, o.M33);
int right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0;
float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue;
for (int i = 0; i < hullVertices.Count; i++)
{
float dotX, dotY, dotZ;
Vector3Ex.Dot(ref rightDirection, ref hullVertices.Elements[i], out dotX);
Vector3Ex.Dot(ref upDirection, ref hullVertices.Elements[i], out dotY);
Vector3Ex.Dot(ref backDirection, ref hullVertices.Elements[i], out dotZ);
if (dotX < minX)
{
minX = dotX;
left = i;
}
if (dotX > maxX)
{
maxX = dotX;
right = i;
}
if (dotY < minY)
{
minY = dotY;
down = i;
}
if (dotY > maxY)
{
maxY = dotY;
up = i;
}
if (dotZ < minZ)
{
minZ = dotZ;
forward = i;
}
if (dotZ > maxZ)
{
maxZ = dotZ;
backward = i;
}
}
//Incorporate the collision margin.
Vector3Ex.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection);
Vector3Ex.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection);
Vector3Ex.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection);
var rightElement = hullVertices.Elements[right];
var leftElement = hullVertices.Elements[left];
var upElement = hullVertices.Elements[up];
var downElement = hullVertices.Elements[down];
var backwardElement = hullVertices.Elements[backward];
var forwardElement = hullVertices.Elements[forward];
Vector3Ex.Add(ref rightElement, ref rightDirection, out rightElement);
Vector3Ex.Subtract(ref leftElement, ref rightDirection, out leftElement);
Vector3Ex.Add(ref upElement, ref upDirection, out upElement);
Vector3Ex.Subtract(ref downElement, ref upDirection, out downElement);
Vector3Ex.Add(ref backwardElement, ref backDirection, out backwardElement);
Vector3Ex.Subtract(ref forwardElement, ref backDirection, out forwardElement);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
TransformLocalExtremePoints(ref rightElement, ref upElement, ref backwardElement, ref o, out boundingBox.Max);
TransformLocalExtremePoints(ref leftElement, ref downElement, ref forwardElement, ref o, out boundingBox.Min);
}
public override Vector3f SelectALightPoint(Vector3f rayFrom)
{
int i = (int)(rayFrom.X / rayFrom.Length() * (trigles.Length - 1));
return(trigles[i].SelectALightPoint(rayFrom));
}
private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//Sample the local directions from the matrix, implicitly transposed.
var rightDirection = new System.Numerics.Vector3(o.M11, o.M21, o.M31);
var upDirection = new System.Numerics.Vector3(o.M12, o.M22, o.M32);
var backDirection = new System.Numerics.Vector3(o.M13, o.M23, o.M33);
int right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0;
float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue;
for (int i = 0; i < hullVertices.Count; i++)
{
float dotX, dotY, dotZ;
Vector3Ex.Dot(ref rightDirection, ref hullVertices.Elements[i], out dotX);
Vector3Ex.Dot(ref upDirection, ref hullVertices.Elements[i], out dotY);
Vector3Ex.Dot(ref backDirection, ref hullVertices.Elements[i], out dotZ);
if (dotX < minX)
{
minX = dotX;
left = i;
}
if (dotX > maxX)
{
maxX = dotX;
right = i;
}
if (dotY < minY)
{
minY = dotY;
down = i;
}
if (dotY > maxY)
{
maxY = dotY;
up = i;
}
if (dotZ < minZ)
{
minZ = dotZ;
forward = i;
}
if (dotZ > maxZ)
{
maxZ = dotZ;
backward = i;
}
}
//Incorporate the collision margin.
Vector3Ex.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection);
Vector3Ex.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection);
Vector3Ex.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection);
var rightElement = hullVertices.Elements[right];
var leftElement = hullVertices.Elements[left];
var upElement = hullVertices.Elements[up];
var downElement = hullVertices.Elements[down];
var backwardElement = hullVertices.Elements[backward];
var forwardElement = hullVertices.Elements[forward];
Vector3Ex.Add(ref rightElement, ref rightDirection, out rightElement);
Vector3Ex.Subtract(ref leftElement, ref rightDirection, out leftElement);
Vector3Ex.Add(ref upElement, ref upDirection, out upElement);
Vector3Ex.Subtract(ref downElement, ref upDirection, out downElement);
Vector3Ex.Add(ref backwardElement, ref backDirection, out backwardElement);
Vector3Ex.Subtract(ref forwardElement, ref backDirection, out forwardElement);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
TransformLocalExtremePoints(ref rightElement, ref upElement, ref backwardElement, ref o, out boundingBox.Max);
TransformLocalExtremePoints(ref leftElement, ref downElement, ref forwardElement, ref o, out boundingBox.Min);
}
public static float DistanceFromSol(System.Numerics.Vector3 point) => point.Length();
