// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp //public class Hull m_optionalHull; public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) { Vector3 supVec = new Vector3(); float maxDot = -1e30f; float lenSqr = vec.LengthSquared(); if (lenSqr < 0.0001f) { vec = new Vector3(1, 0, 0); } else { float rlen = 1f / (float)Math.Sqrt(lenSqr); vec *= rlen; } Vector3 vtx; float newDot; for (int i = 0; i < VertexCount; i++) { GetVertex(i, out vtx); newDot = Vector3.Dot(vec, vtx); if (newDot > maxDot) { maxDot = newDot; supVec = vtx; } } return supVec; }
public void SetForce(Vector3 force) { if (force.LengthSquared() > 1.0f) force.Normalize(); mController.Force = GameOptions.MovementForce * force; }
/// <summary> /// /// </summary> /// <param name="value"></param> /// <param name="max"></param> public static Vector3 Truncate(Vector3 value, float max) { float maxSquared = max * max; float vectorSquared = value.LengthSquared(); if (vectorSquared <= maxSquared) return value; else return value * (max / (float)Math.Sqrt(vectorSquared)); }
public static void ProjectVector3D(ref Vector3 vec, ref Vector3 projectOn, out Vector3 result) { float dp; Vector3.Dot(ref vec, ref projectOn, out dp); float oneOnLenSqr = 1.0f / projectOn.LengthSquared(); result = new Vector3( dp * oneOnLenSqr * projectOn.X, dp * oneOnLenSqr * projectOn.Y, dp * oneOnLenSqr * projectOn.Y); }
public static Vector3 Extend(Vector3 value, float min) { float minSquared = min*min; float vectorSquared = value.LengthSquared(); if (vectorSquared >= minSquared || vectorSquared == 0) return value; else return value*(min/(float) Math.Sqrt(vectorSquared)); }
private void FaceDirectionMoving(Vector3 directionMoved) { if (directionMoved.LengthSquared() != 0) { var angle = (float)Math.Atan2(directionMoved.Y, directionMoved.X); // Up is an angle of 0, so we need to offset by pi/2 this.RotationZ = angle + MathHelper.PiOver2; } }
public void update() { velocity += _acceleration; position += velocity; _acceleration = Vector3.Zero; if( velocity.LengthSquared() < 0.001f * 0.001f ) velocity = Vector3.Zero; velocity *= _damping; _damping = 0.98f; }
///<summary> /// Gets the extreme point of the shape in world space in a given direction with margin expansion. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePoint(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint) { GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint); float directionLength = direction.LengthSquared(); if (directionLength > Toolbox.Epsilon) { Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3.Add(ref extremePoint, ref direction, out extremePoint); } }
public MovingLevelPiece(Game game, GameplayScreen host, String assetName, Vector3 pMovement, float pMoveSpeed) : base(game, host, assetName) { m_movement = pMovement; m_moveVelocity = m_movement; if (m_moveVelocity.LengthSquared() != 0) { m_moveVelocity.Normalize(); } //50 is a good number, fyi m_moveVelocity *= pMoveSpeed; m_currentMovement = Vector3.Zero; }
public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) { Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); Quaternion dorn = MathHelper.GetRotation(dmat); angle = 2f * (float)Math.Acos(dorn.W); axis = new Vector3(dorn.X, dorn.Y, dorn.Z); //check for axis length float len = axis.LengthSquared(); if (len < MathHelper.Epsilon * MathHelper.Epsilon) axis = new Vector3(1f, 0f, 0f); else axis /= (float)Math.Sqrt(len); }
/// <summary> /// 正規化された Vector3 を取得します。 /// もしも Vector3 がゼロ ベクトルである場合にはゼロ ベクトルを返します。 /// </summary> /// <param name="vector">Vector3。</param> /// <param name="result">正規化された Vector3。</param> public static void NormalizeSafe(ref Vector3 vector, out Vector3 result) { var lengthSquared = vector.LengthSquared(); if (lengthSquared != 0.0f) { var coeff = 1.0f / (float) Math.Sqrt(lengthSquared); result.X = vector.X * coeff; result.Y = vector.Y * coeff; result.Z = vector.Z * coeff; } else { result = vector; } }
public override bool contains(Vector3 point) { Vector3 _p = new Vector3(point.X - Position.X, point.Y - Position.Y, point.Z - Position.Z); //Vector3 _p2 = Vector3.Transform(_p, Matrix.CreateFromQuaternion(Orientation)); if (_p.Y >= 0 && _p.Y <= m_height) { if (_p.LengthSquared() < (m_radius * m_radius)) { return true; } } return false; }
public InertialMoveBehavior(Vector3 v, float a, float mv) : base() { MaxSpeed = mv; Accel = a; if (v.LengthSquared() == 0) { Speed = 0; Direction = new Vector3(); } else { Speed = v.Length(); v.Normalize(); Direction = v; } }
public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) { linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); Vector3 axis; float angle = 2 * (float)Math.Acos(dorn.W); axis = new Vector3(dorn.X, dorn.Y, dorn.Z); //axis[3] = 0.f; //check for axis length float len = axis.LengthSquared(); if (len < MathHelper.Epsilon * MathHelper.Epsilon) axis = new Vector3(1f, 0f, 0f); else axis /= (float)Math.Sqrt(len); angularVelocity = axis * angle / timeStep; }
/// <summary> /// Moves the constraint lines to the proper location relative to the entities involved. /// </summary> public override void Update() { //Move lines around PointOnPlaneJoint constraint = LineObject; Microsoft.Xna.Framework.Vector3 planeAnchor = MathConverter.Convert(constraint.PlaneAnchor); Microsoft.Xna.Framework.Vector3 y = MathConverter.Convert(Vector3.Cross(constraint.ConnectionA.OrientationMatrix.Up, constraint.PlaneNormal)); if (y.LengthSquared() < .001f) { y = MathConverter.Convert(Vector3.Cross(constraint.ConnectionA.OrientationMatrix.Right, constraint.PlaneNormal)); } Microsoft.Xna.Framework.Vector3 x = Microsoft.Xna.Framework.Vector3.Cross(MathConverter.Convert(constraint.PlaneNormal), y); //Grid gridRow1.PositionA = planeAnchor - 1.5f * x + y; gridRow1.PositionB = planeAnchor + 1.5f * x + y; gridRow2.PositionA = planeAnchor - 1.5f * x; gridRow2.PositionB = planeAnchor + 1.5f * x; gridRow3.PositionA = planeAnchor - 1.5f * x - y; gridRow3.PositionB = planeAnchor + 1.5f * x - y; gridColumn1.PositionA = planeAnchor + x - 1.5f * y; gridColumn1.PositionB = planeAnchor + x + 1.5f * y; gridColumn2.PositionA = planeAnchor - 1.5f * y; gridColumn2.PositionB = planeAnchor + 1.5f * y; gridColumn3.PositionA = planeAnchor - x - 1.5f * y; gridColumn3.PositionB = planeAnchor - x + 1.5f * y; //Connection and error aToConnection.PositionA = MathConverter.Convert(constraint.ConnectionA.Position); aToConnection.PositionB = MathConverter.Convert(constraint.PlaneAnchor); bToConnection.PositionA = MathConverter.Convert(constraint.ConnectionB.Position); bToConnection.PositionB = MathConverter.Convert(constraint.PointAnchor); error.PositionA = aToConnection.PositionB; error.PositionB = bToConnection.PositionB; }
///<summary> /// Gets the extreme point of the shape in local space in a given direction. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { //Is it the tip of the cone? float sinThetaSquared = radius * radius / (radius * radius + height * height); //If d.Y * d.Y / d.LengthSquared >= sinthetaSquared if (direction.Y > 0 && direction.Y * direction.Y >= direction.LengthSquared() * sinThetaSquared) { extremePoint = new Vector3(0, .75f * height, 0); return; } //Is it a bottom edge of the cone? float horizontalLengthSquared = direction.X * direction.X + direction.Z * direction.Z; if (horizontalLengthSquared > Toolbox.Epsilon) { var radOverSigma = radius / Math.Sqrt(horizontalLengthSquared); extremePoint = new Vector3((float)(radOverSigma * direction.X), -.25f * height, (float)(radOverSigma * direction.Z)); } else // It's pointing almost straight down... extremePoint = new Vector3(0, -.25f * height, 0); }
public void Update() { Velocity += acceleration; Position += Velocity; acceleration = Vector3.Zero; if (Velocity.LengthSquared() < 0.001f * 0.001f) Velocity = Vector3.Zero; Velocity *= damping; damping = 0.98f; }
public static Vector3 Project(this Vector3 v, Vector3 on) { return v.Dot(on) / on.LengthSquared() * on; }
public void Update(float elapsedTime) { Vector3 movementDir = new Vector3(); if (this.MovementEnabled) { Vector2 controller = this.main.Camera.GetWorldSpaceControllerCoordinates(this.Movement); movementDir = new Vector3(controller.X, 0, controller.Y); if (this.Up) movementDir = movementDir.SetComponent(Direction.PositiveY, 1.0f); else if (this.Down) movementDir = movementDir.SetComponent(Direction.NegativeY, 1.0f); if (this.MapEditMode) { bool moving = movementDir.LengthSquared() > 0.0f; // When the user lets go of the key, reset the timer // That way they can hit the key faster than the 0.1 sec interval if (!moving) this.movementInterval = 0.5f; if (this.movementInterval > (this.SpeedMode ? 0.05f : 0.1f)) { if (moving) this.movementInterval = 0.0f; if (movementDir.LengthSquared() > 0.0f) { Map map = this.SelectedEntities[0].Get<Map>(); Direction relativeDir = map.GetRelativeDirection(movementDir); this.coord = this.coord.Move(relativeDir); if (this.EditSelection) { this.VoxelSelectionStart.Value = new Map.Coordinate { X = Math.Min(this.selectionStart.X, this.coord.X), Y = Math.Min(this.selectionStart.Y, this.coord.Y), Z = Math.Min(this.selectionStart.Z, this.coord.Z), }; this.VoxelSelectionEnd.Value = new Map.Coordinate { X = Math.Max(this.selectionStart.X, this.coord.X) + 1, Y = Math.Max(this.selectionStart.Y, this.coord.Y) + 1, Z = Math.Max(this.selectionStart.Z, this.coord.Z) + 1, }; } else if (this.TransformMode.Value == TransformModes.Translate) { this.NeedsSave.Value = true; this.restoreMap(this.VoxelSelectionStart, this.VoxelSelectionEnd, !this.voxelDuplicate); Map.Coordinate newSelectionStart = this.VoxelSelectionStart.Value.Move(relativeDir); this.VoxelSelectionStart.Value = newSelectionStart; this.VoxelSelectionEnd.Value = this.VoxelSelectionEnd.Value.Move(relativeDir); this.mapState.Add(map.GetChunksBetween(this.VoxelSelectionStart, this.VoxelSelectionEnd)); Map.Coordinate offset = this.originalSelectionStart.Minus(newSelectionStart); this.restoreMap(newSelectionStart, this.VoxelSelectionEnd, false, offset.X, offset.Y, offset.Z); } this.Position.Value = map.GetAbsolutePosition(this.coord); } } this.movementInterval += elapsedTime; } else this.Position.Value = this.Position.Value + movementDir * (this.SpeedMode ? 50.0f : 25.0f) * elapsedTime; } if (this.MapEditMode) { if (!this.Fill && !this.Empty) this.justCommitedOrRevertedVoxelOperation = false; Map map = this.SelectedEntities[0].Get<Map>(); Map.Coordinate coord = map.GetCoordinate(this.Position); if (this.TransformMode.Value == TransformModes.None && (this.Fill || this.Empty || this.Extend) && !this.justCommitedOrRevertedVoxelOperation) { this.NeedsSave.Value = true; if (this.Brush == "[Procedural]") { ProceduralGenerator generator = this.Entity.Get<ProceduralGenerator>(); if (this.Fill) { if (this.VoxelSelectionActive) { foreach (Map.Coordinate c in this.VoxelSelectionStart.Value.CoordinatesBetween(this.VoxelSelectionEnd)) map.Fill(c, generator.GetValue(map, c)); } else this.brushStroke(map, coord, this.BrushSize, x => generator.GetValue(map, x), true, false); } else if (this.Empty) { if (this.VoxelSelectionActive) map.Empty(this.VoxelSelectionStart.Value.CoordinatesBetween(this.VoxelSelectionEnd).Where(x => generator.GetValue(map, x).ID == 0)); else this.brushStroke(map, coord, this.BrushSize, x => generator.GetValue(map, x), false, true); } } else { if (this.Fill) { Map.CellState material; if (WorldFactory.StatesByName.TryGetValue(this.Brush, out material)) { if (this.VoxelSelectionActive) map.Fill(this.VoxelSelectionStart, this.VoxelSelectionEnd, material); else this.brushStroke(map, coord, this.BrushSize, material); } } else if (this.Empty) { if (this.VoxelSelectionActive) map.Empty(this.VoxelSelectionStart, this.VoxelSelectionEnd); else this.brushStroke(map, coord, this.BrushSize, new Map.CellState()); } } if (this.Extend && !this.coord.Equivalent(this.lastCoord)) { Direction dir = DirectionExtensions.GetDirectionFromVector(Vector3.TransformNormal(movementDir, Matrix.Invert(map.Transform))); Map.Box box = map.GetBox(this.lastCoord); bool grow = map.GetBox(this.coord) != box; if (box != null) { List<Map.Coordinate> removals = new List<Map.Coordinate>(); if (dir.IsParallel(Direction.PositiveX)) { for (int y = box.Y; y < box.Y + box.Height; y++) { for (int z = box.Z; z < box.Z + box.Depth; z++) { if (grow) map.Fill(this.coord.X, y, z, box.Type); else removals.Add(map.GetCoordinate(this.lastCoord.X, y, z)); } } } if (dir.IsParallel(Direction.PositiveY)) { for (int x = box.X; x < box.X + box.Width; x++) { for (int z = box.Z; z < box.Z + box.Depth; z++) { if (grow) map.Fill(x, this.coord.Y, z, box.Type); else removals.Add(map.GetCoordinate(x, this.lastCoord.Y, z)); } } } if (dir.IsParallel(Direction.PositiveZ)) { for (int x = box.X; x < box.X + box.Width; x++) { for (int y = box.Y; y < box.Y + box.Height; y++) { if (grow) map.Fill(x, y, this.coord.Z, box.Type); else removals.Add(map.GetCoordinate(x, y, this.lastCoord.Z)); } } } map.Empty(removals); } } map.Regenerate(); } this.lastCoord = this.coord; } else if (this.TransformMode.Value == TransformModes.Translate) { // Translate entities this.NeedsSave.Value = true; float rayLength = (this.transformCenter - this.main.Camera.Position.Value).Length(); Vector2 mouseOffset = this.Mouse - this.originalTransformMouse; Vector3 offset = ((this.main.Camera.Right.Value * mouseOffset.X * rayLength) + (this.main.Camera.Up.Value * -mouseOffset.Y * rayLength)) * 0.0025f; switch (this.TransformAxis.Value) { case TransformAxes.X: offset.Y = offset.Z = 0.0f; break; case TransformAxes.Y: offset.X = offset.Z = 0.0f; break; case TransformAxes.Z: offset.X = offset.Y = 0.0f; break; } if (this.SelectedTransform.Value != null) this.SelectedTransform.Value.Position.Value = this.offsetTransforms[0].Translation + offset; else { int i = 0; foreach (Entity entity in this.SelectedEntities) { Transform transform = entity.Get<Transform>(); if (transform != null) { Matrix originalTransform = this.offsetTransforms[i]; transform.Position.Value = originalTransform.Translation + offset; i++; } } } } else if (this.TransformMode.Value == TransformModes.Rotate) { // Rotate entities this.NeedsSave.Value = true; Vector3 screenSpaceCenter = this.main.GraphicsDevice.Viewport.Project(this.transformCenter, this.main.Camera.Projection, this.main.Camera.View, Matrix.Identity); Vector2 originalOffset = new Vector2(this.originalTransformMouse.X - screenSpaceCenter.X, this.originalTransformMouse.Y - screenSpaceCenter.Y); float originalAngle = (float)Math.Atan2(originalOffset.Y, originalOffset.X); Vector2 newOffset = new Vector2(this.Mouse.Value.X - screenSpaceCenter.X, this.Mouse.Value.Y - screenSpaceCenter.Y); float newAngle = (float)Math.Atan2(newOffset.Y, newOffset.X); Vector3 axis = this.main.Camera.Forward; switch (this.TransformAxis.Value) { case TransformAxes.X: axis = Vector3.Right; break; case TransformAxes.Y: axis = Vector3.Up; break; case TransformAxes.Z: axis = Vector3.Forward; break; } if (this.SelectedTransform.Value != null) { Matrix originalTransform = this.offsetTransforms[0]; originalTransform.Translation -= this.transformCenter; originalTransform *= Matrix.CreateFromAxisAngle(axis, newAngle - originalAngle); originalTransform.Translation += this.transformCenter; this.SelectedTransform.Value.Matrix.Value = originalTransform; } else { int i = 0; foreach (Entity entity in this.SelectedEntities) { Transform transform = entity.Get<Transform>(); if (transform != null) { Matrix originalTransform = this.offsetTransforms[i]; originalTransform.Translation -= this.transformCenter; originalTransform *= Matrix.CreateFromAxisAngle(axis, newAngle - originalAngle); originalTransform.Translation += this.transformCenter; transform.Matrix.Value = originalTransform; i++; } } } } }
public void Update(GameTime a_GameTime) { float dtSpeed = ((float)a_GameTime.ElapsedGameTime.Milliseconds / 1000.0f) * 1000.0f; // move player Vector3 addativeDirection = new Vector3(0); if (Keyboard.GetState().IsKeyDown(Keys.Left) || Keyboard.GetState().IsKeyDown(Keys.A)) addativeDirection = new Vector3(-1.0f, 0, 0); else if (Keyboard.GetState().IsKeyDown(Keys.Right) || Keyboard.GetState().IsKeyDown(Keys.D)) addativeDirection = new Vector3(1.0f, 0, 0); if (Keyboard.GetState().IsKeyDown(Keys.Up) || Keyboard.GetState().IsKeyDown(Keys.W)) addativeDirection += new Vector3(0, -1.0f, 0); else if (Keyboard.GetState().IsKeyDown(Keys.Down) || Keyboard.GetState().IsKeyDown(Keys.S)) addativeDirection += new Vector3(0, 1.0f, 0); if (m_PrevKeyboardState.IsKeyUp(Keys.Space) && Keyboard.GetState().IsKeyDown(Keys.Space)) { ShootBullet(); } else if (Keyboard.GetState().IsKeyDown(Keys.Space)) { m_ShootDT -= a_GameTime.ElapsedGameTime.Milliseconds; if (m_ShootDT <= 0) { ShootBullet(); m_ShootDT = 200.0f; } } if( addativeDirection.LengthSquared() != 0) addativeDirection.Normalize(); Velocity += addativeDirection * dtSpeed; Velocity += Acceleration * a_GameTime.ElapsedGameTime.Milliseconds * 0.001f; Position += Velocity * a_GameTime.ElapsedGameTime.Milliseconds * 0.001f; // rotate player if (Velocity.LengthSquared() != 0) { float diff = ((float)Math.Atan2(Velocity.X, Velocity.Y) - Rotation); if (diff >= MathHelper.ToRadians(180) || diff <= -MathHelper.ToRadians(180)) Rotation *= -1; Rotation += diff * 0.1f; } Velocity *= 0.95f; m_PrevKeyboardState = Keyboard.GetState(); }
private static Vector3 contactPoint(Vector3 pOne, Vector3 dOne, float oneSize, Vector3 pTwo, Vector3 dTwo, float twoSize, // If this is true, and the contact point is outside // the edge (in the case of an edge-face contact) then // we use one's midpoint, otherwise we use two's. bool useOne) { Vector3 toSt, cOne, cTwo; float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo; float denom, mua, mub; smOne = dOne.LengthSquared(); smTwo = dTwo.LengthSquared(); dpOneTwo = Vector3.Dot(dTwo, dOne); toSt = pOne - pTwo; dpStaOne = Vector3.Dot(dOne, toSt); dpStaTwo = Vector3.Dot(dTwo, toSt); denom = smOne * smTwo - dpOneTwo * dpOneTwo; // Zero denominator indicates parrallel lines if (Math.Abs(denom) < 0.0001f) { return useOne ? pOne : pTwo; } mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom; mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom; // If either of the edges has the nearest point out // of bounds, then the edges aren't crossed, we have // an edge-face contact. Our point is on the edge, which // we know from the useOne parameter. if (mua > oneSize || mua < -oneSize || mub > twoSize || mub < -twoSize) { return useOne ? pOne : pTwo; } else { cOne = pOne + dOne * mua; cTwo = pTwo + dTwo * mub; return Vector3.Multiply(cOne, 0.5f) + Vector3.Multiply(cTwo, 0.5f); } }
private bool tryAxis(Box one, Box two, Vector3 axis, Vector3 toCentre, int index, // These values may be updated ref float smallestPenetration, ref int smallestCase) { if (axis.LengthSquared() < 0.0001) return true; axis.Normalize(); float penetration = penetrationOnAxis(one, two, axis, toCentre); if (penetration < 0) // there is no Contact return false; if (penetration < smallestPenetration) // Set the Smallest { smallestPenetration = penetration; smallestCase = index; } return true; }
/// <summary> /// Determines the location of the point when projected onto the plane defined by the normal and a point on the plane. /// </summary> /// <param name="point">Point to project onto plane.</param> /// <param name="normal">Normal of the plane.</param> /// <param name="pointOnPlane">Point located on the plane.</param> /// <param name="projectedPoint">Projected location of point onto plane.</param> public static void GetPointProjectedOnPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane, out Vector3 projectedPoint) { float dot; Vector3.Dot(ref normal, ref point, out dot); float dot2; Vector3.Dot(ref pointOnPlane, ref normal, out dot2); float t = (dot - dot2) / normal.LengthSquared(); Vector3 multiply; Vector3.Multiply(ref normal, t, out multiply); Vector3.Subtract(ref point, ref multiply, out projectedPoint); }
/// <summary> /// Creates the smallest <see cref="BoundingSphere"/> that can contain a specified list of points in 3D-space. /// </summary> /// <param name="points">List of point to create the sphere from.</param> /// <returns>The new <see cref="BoundingSphere"/>.</returns> public static BoundingSphere CreateFromPoints(IEnumerable <Vector3> points) { if (points == null) { throw new ArgumentNullException("points"); } // From "Real-Time Collision Detection" (Page 89) Vector3 minx = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue); Vector3 maxx = -minx; Vector3 miny = minx; Vector3 maxy = -minx; Vector3 minz = minx; Vector3 maxz = -minx; // Find the most extreme points along the principle axis. int numPoints = 0; foreach (Vector3 pt in points) { numPoints += 1; if (pt.X < minx.X) { minx = pt; } if (pt.X > maxx.X) { maxx = pt; } if (pt.Y < miny.Y) { miny = pt; } if (pt.Y > maxy.Y) { maxy = pt; } if (pt.Z < minz.Z) { minz = pt; } if (pt.Z > maxz.Z) { maxz = pt; } } if (numPoints == 0) { throw new ArgumentException( "You should have at least one point in points." ); } float sqDistX = Vector3.DistanceSquared(maxx, minx); float sqDistY = Vector3.DistanceSquared(maxy, miny); float sqDistZ = Vector3.DistanceSquared(maxz, minz); // Pick the pair of most distant points. Vector3 min = minx; Vector3 max = maxx; if (sqDistY > sqDistX && sqDistY > sqDistZ) { max = maxy; min = miny; } if (sqDistZ > sqDistX && sqDistZ > sqDistY) { max = maxz; min = minz; } Vector3 center = (min + max) * 0.5f; float radius = Vector3.Distance(max, center); // Test every point and expand the sphere. // The current bounding sphere is just a good approximation and may not enclose all points. // From: Mathematics for 3D Game Programming and Computer Graphics, Eric Lengyel, Third Edition. // Page 218 float sqRadius = radius * radius; foreach (Vector3 pt in points) { Vector3 diff = (pt - center); float sqDist = diff.LengthSquared(); if (sqDist > sqRadius) { float distance = (float)Math.Sqrt(sqDist); // equal to diff.Length(); Vector3 direction = diff / distance; Vector3 G = center - radius * direction; center = (G + pt) / 2; radius = Vector3.Distance(pt, center); sqRadius = radius * radius; } } return(new BoundingSphere(center, radius)); }
/// <summary> /// Damp steering force when moving slowly /// </summary> /// <param name="steering"></param> /// <returns></returns> public Vector3 adjustRawSteeringForce(Vector3 steering) { float maxAdjustedSpeed = 0.2f * MaxSpeed; if ((Speed > maxAdjustedSpeed) || steering.LengthSquared() < float.Epsilon) { return steering; } else { float range = Speed / maxAdjustedSpeed; float cosine = MathHelper.Lerp(1.0f, -1.0f, (float)Math.Pow(range, 10)); return ClipSteering(steering, cosine, Forward); } }
/// <summary> /// Update the IR distance sensor /// </summary> /// <param name="update"></param> public override void Update(simengine.FrameUpdate update) { base.Update(update); _elapsedSinceLastScan += (float)update.ElapsedTime; _appTime = (float)update.ApplicationTime; // only retrieve raycast results every SCAN_INTERVAL. // For entities that are compute intenisve, you should consider giving them // their own task queue so they dont flood a shared queue if ((_elapsedSinceLastScan > SCAN_INTERVAL) && (_raycastProperties != null)) { _elapsedSinceLastScan = 0; // The default pose has the IR sensor looking toward the back of the robot. Rotate // it by 180 degrees. _raycastProperties.OriginPose.Orientation = simengine.TypeConversion.FromXNA( simengine.TypeConversion.ToXNA(Parent.State.Pose.Orientation) * simengine.TypeConversion.ToXNA(State.Pose.Orientation)); _raycastProperties.OriginPose.Position = simengine.TypeConversion.FromXNA( xna.Vector3.Transform(simengine.TypeConversion.ToXNA(State.Pose.Position), Parent.World)); xna.Matrix orientation = xna.Matrix.CreateFromQuaternion(simengine.TypeConversion.ToXNA(State.Pose.Orientation)); World = xna.Matrix.Multiply(orientation, xna.Matrix.CreateTranslation(simengine.TypeConversion.ToXNA(State.Pose.Position))); // This entity is relative to its parent World = xna.Matrix.Multiply(World, Parent.World); // cast rays on a horizontal plane and again on a vertical plane _raycastResultsPort = PhysicsEngine.Raycast2D(_raycastProperties); _raycastResultsPort.Test(out _lastResults); if (_lastResults != null) { simcommon.RaycastResult verticalResults; // rotate the plane by 90 degrees _raycastProperties.OriginPose.Orientation = simengine.TypeConversion.FromXNA(simengine.TypeConversion.ToXNA(_raycastProperties.OriginPose.Orientation) * xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 0, 1), (float)Math.PI / 2f)); _raycastResultsPort = PhysicsEngine.Raycast2D(_raycastProperties); _raycastResultsPort.Test(out verticalResults); // combine the results of the second raycast with the first if (verticalResults != null) { foreach (simcommon.RaycastImpactPoint impact in verticalResults.ImpactPoints) { _lastResults.ImpactPoints.Add(impact); } } // find the shortest distance to an impact point float minRange = MaximumRange * MaximumRange; xna.Vector4 origin = new xna.Vector4(simengine.TypeConversion.ToXNA(_raycastProperties.OriginPose.Position), 1); foreach (simcommon.RaycastImpactPoint impact in _lastResults.ImpactPoints) { xna.Vector3 impactVector = new xna.Vector3( impact.Position.X - origin.X, impact.Position.Y - origin.Y, impact.Position.Z - origin.Z); float impactDistanceSquared = impactVector.LengthSquared(); if (impactDistanceSquared < minRange) { minRange = impactDistanceSquared; } } _distance = (float)Math.Sqrt(minRange); } } }
protected virtual void SetAtmosphereEffectParameters(Matrix View, Matrix Projection, Vector3 CameraPosition) { float scale = 1.0f / (a_radius - p_radius); atmosphere.Parameters["fOuterRadius"].SetValue(a_radius); atmosphere.Parameters["fInnerRadius"].SetValue(p_radius); atmosphere.Parameters["fOuterRadius2"].SetValue(a_radius * a_radius); atmosphere.Parameters["fInnerRadius2"].SetValue(p_radius * p_radius); atmosphere.Parameters["fKr4PI"].SetValue(0.0025f * 4 * MathHelper.Pi); atmosphere.Parameters["fKm4PI"].SetValue(0.0015f * 4 * MathHelper.Pi); atmosphere.Parameters["fScale"].SetValue(scale); atmosphere.Parameters["fScaleDepth"].SetValue(0.25f); atmosphere.Parameters["fScaleOverScaleDepth"].SetValue(scale / 0.25f); atmosphere.Parameters["fSamples"].SetValue(2.0f); atmosphere.Parameters["nSamples"].SetValue(2); // ここを惑星ごとに変えるべし SetAtmosphereEffectParametersDetail(); Matrix World = Matrix.CreateScale(a_radius) * Matrix.CreateRotationX(pitch) * Matrix.CreateTranslation(Position); World.Translation = Position; //Vector3 vl = -level.LightPosition; Vector3 vl = -level.LightPosition; vl.Normalize(); atmosphere.Parameters["World"].SetValue(World); World = Matrix.CreateScale(a_radius) * Matrix.CreateRotationX(pitch); atmosphere.Parameters["DefWorld"].SetValue(World); atmosphere.Parameters["View"].SetValue(View); atmosphere.Parameters["Projection"].SetValue(Projection); atmosphere.Parameters["v3CameraPos"].SetValue(CameraPosition); atmosphere.Parameters["v3LightDir"].SetValue(vl); atmosphere.Parameters["v3LightPos"].SetValue(level.LightPosition); atmosphere.Parameters["fCameraHeight"].SetValue(CameraPosition.Length()); atmosphere.Parameters["fCameraHeight2"].SetValue(CameraPosition.LengthSquared()); }
private Vector3 projectOnSphere(Vector2 coord) { Rectangle rec = _game.Window.ClientBounds; Vector3 res = new Vector3(coord.X/_screenCenter.X, coord.Y/_screenCenter.Y, 0); res -= new Vector3(1, 1, 0); res.Y = -res.Y; res.Z = 1 - res.LengthSquared(); res.Z = res.Z > 0 ? (float)Math.Sqrt(res.Z) : 0; res.Normalize(); return res; }
/// <summary> /// Calculates the vector that results when a vector is projected onto another. (the vector component of one vector along another) /// </summary> /// <param name="projectedVector">The vector that is being projected.</param> /// <param name="vectorProjectedOnto">The vector that is being projected onto.</param> public static Vector3 Projection(Vector3 projectedVector, Vector3 vectorProjectedOnto) { return (Vector3.Dot(projectedVector, vectorProjectedOnto) / vectorProjectedOnto.LengthSquared()) * vectorProjectedOnto; }
public static bool VectorsDirectionEqual(Vector3 vec1, Vector3 vec2, float allowedError) { float len1 = vec1.LengthSquared(); float len2 = vec2.LengthSquared(); float dot = Vector3.Dot(vec1, vec2); if(!(dot<0.0f)) dot *= dot / (len1 * len2); return dot < 0.0f ? false : 1.0f - dot < allowedError; }
/// <summary> /// Determines the distance between a point and a plane.. /// </summary> /// <param name="point">Point to project onto plane.</param> /// <param name="normal">Normal of the plane.</param> /// <param name="pointOnPlane">Point located on the plane.</param> /// <returns>Distance from the point to the plane.</returns> public static float GetDistancePointToPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane) { Vector3 offset; Vector3.Subtract(ref point, ref pointOnPlane, out offset); float dot; Vector3.Dot(ref normal, ref offset, out dot); return dot / normal.LengthSquared(); }
public static float ScalarProjection(Vector3 A, Vector3 B) { return Vector3.Dot(A, B) / B.LengthSquared(); }
void ModifySpeed(float scale) { float speed = 16.0f; float dt = FrameController.DT(); scale *= thumbstickMagnitude; // scale the supposed speed on the thumbstick mag Vector3 dir = playerCamera.transform.GetForwardVector(); dir.Y = 0; dir.Normalize(); // direction for z axis Vector3 right = playerCamera.transform.GetRightVector(); right.Y = 0; right.Normalize(); // direction for x axis Vector3 currVel = playerPhysics.rigidCom.Body().LinearVelocity(); float currentYVelocity = currVel.Y; Vector3 newVel = new Vector3(); // has no y component here newVel = right * speedX; // update X newVel += dir * speedZ; // update Z newVel.Normalize(); newVel = newVel * scale * speed; // scalar for velocity (less speed in air) if (smc.GetCurrentState() == jump) { float limitSpeedInJump = speedWhenJumped * speed; newVel = currVel + newVel * dt; newVel.X = MathHelper.Clamp(newVel.X, -limitSpeedInJump, limitSpeedInJump); newVel.Z = MathHelper.Clamp(newVel.Z, -limitSpeedInJump, limitSpeedInJump); } else { newVel.Y = 0;//currentYVelocity; } playerPhysics.rigidCom.Body().SetLinearVelocity(newVel * dt * 30.0f); #if ZERO float max = 33.0f; // cap it to max if (newVel.LengthSquared() < max * max) { newVel.Y = yVel; playerPhysics.rigidCom.Body().SetLinearVelocity(newVel); } else { newVel.Normalize(); newVel *= max; newVel.Y = yVel; playerPhysics.rigidCom.Body().SetLinearVelocity(newVel); } #endif }
/// <summary> /// Create the particle systems /// </summary> void CreateParticles() { Vector3 randomVelocity; float lengthSquared; // for each particle for( int i=0;i<count; i++ ) { // get an equally distributed random direction do { randomVelocity = new Vector3( (float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f); lengthSquared = randomVelocity.LengthSquared(); // if outside sphere get another sample } while (lengthSquared > 1.0f); // add to the Z direction for a cone like emmission randomVelocity.Z += emissionAngle; // normalize direction randomVelocity = Vector3.Normalize(randomVelocity); // normalized random number float randomNumber = (float)random.NextDouble(); // velocity vector from range of min and max values randomVelocity *= velocity.X * randomNumber + velocity.Y * (1.0f - randomNumber); // random scale and time offset // (zero time offset to emitt all particles at same time) Vector2 randomVector = new Vector2((float)random.NextDouble(), loop ? (float)random.NextDouble() : 0); // add the particle to the list particles.Add(new Particle(Vector3.Zero, randomVelocity, randomVector)); } }