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