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