public void Move(Vector3 dir, float amount)
{
if (dir.Length() != 1 && dir.Length() != 0)
dir.Normalize();
Position += Vector3.Transform(dir * amount, Matrix.CreateRotationX(Pitch) * Matrix.CreateRotationY(Yaw));
}
/// <summary>
/// Creates a new, cool, advanced Model. For Accelerated Delivery. Otherwise I get
/// lots of silly repetative variables. Note that all rendering options are hardcoded.
/// </summary>
/// <param name="machineNo">The machine number. For any numbers outside of 1-10, it is treated as the number of
/// milliseconds until this machine automatically activates.</param>
/// <param name="machines">An array of machines this machine holds. Null is acceptable.</param>
/// <param name="models">A list of BaseModels and Tubes that the Machine contains.</param>
/// <param name="targetPos">The amount of X, Y, and Z to translate.</param>
/// <param name="timestep">The amount of time to use to translate.</param>
public TranslateMachine(int machineNo, int soundIndex, Vector3 targetPos, float timestep, bool automatic, params BaseModel[] models)
: base(machineNo, soundIndex, models)
{
joints = new List<WeldJoint>();
timeStep = timestep;
targetPosition = targetPos;
this.automatic = automatic;
baseJoint = new PrismaticJoint(null, modelList[0].Ent, modelList[0].Ent.Position, Vector3.Normalize(targetPos), modelList[0].Ent.Position);
baseJoint.Motor.IsActive = true;
baseJoint.Motor.Settings.Mode = MotorMode.Servomechanism;
baseJoint.Motor.Settings.Servo.Goal = 0;
baseJoint.Limit.Maximum = targetPos.Length();
baseJoint.Limit.Minimum = 0;
baseJoint.Limit.IsActive = true;
baseJoint.Motor.Settings.Servo.BaseCorrectiveSpeed = targetPos.Length() / timestep;
baseJoint.Motor.Settings.Servo.MaxCorrectiveVelocity = targetPos.Length() / timestep;
baseJoint.Motor.Settings.Servo.SpringSettings.StiffnessConstant = 0;
baseJoint.Motor.Settings.Servo.SpringSettings.DampingConstant /= 15;
foreach(BaseModel m in modelList)
{
if(m == modelList[0])
continue;
WeldJoint j = new WeldJoint(modelList[0].Ent, m.Ent);
joints.Add(j);
}
foreach(Tube t in tubeList)
t.SetParent(modelList[0].Ent);
if(automatic)
{
inputs.Clear();
inputs.Add(new MachineTimer(machineNo / 1000f));
}
}
private void KeyboardControl()
{
if (Input.Y == 1)
{
Velocity += ACCELERATION * Direction;
}
else if (Input.Y == -1)
{
Velocity -= ACCELERATION * Direction;
}
if (Input.X == 1)
{
Velocity += ACCELERATION * Right;
}
else if (Input.X == -1)
{
Velocity -= ACCELERATION * Right;
}
else if (InputManager.IsKeyDown(Keys.Space))
{
Velocity += Up * ACCELERATION;
}
else if (InputManager.IsKeyDown(Keys.LeftShift))
{
Velocity -= Up * ACCELERATION;
}
if (Input.Y == 0)
{
var dir = Direction;
dir.Y = 0;
var dot = Vector3.Dot(Velocity, Vector3.Normalize(Direction));
Velocity -= Math.Sign(dot) * Direction * DECELERATION;
}
if (Input.X == 0)
{
var dir = Direction;
dir.Y = 0;
var vel = Velocity;
vel.Y = 0;
var dot = Vector3.Dot(Velocity, Vector3.Normalize(Right));
Velocity -= Math.Sign(dot) * Right * DECELERATION;
}
if (Velocity.Length() < 1f && Input.X == 0 && Input.Y == 0)
{
Velocity = new Vector3(0);
}
// MaxSpeed
if (Velocity.Length() > MAX_SPEED)
{
Velocity = Vector3.Normalize(Velocity) * MAX_SPEED;
}
}
public override void Update(GameTime gameTime)
{
KeyboardManager KManager = (KeyboardManager)Game.Services.GetService(typeof(KeyboardManager));
if (KManager.IsKeyPressed(Keys.M))
{
Mode = (CameraMode)(((int)Mode + 1) % 2);
}
float X = KManager.IsKeyDown(Keys.Right) ? 1 : 0;
X -= KManager.IsKeyDown(Keys.Left) ? 1 : 0;
float Z = KManager.IsKeyDown(Keys.Down) ? 1 : 0;
Z -= KManager.IsKeyDown(Keys.Up) ? 1 : 0;
Vector3 move = new Vector3(X, 0, Z);
if (move.Length() > 0)
{
world_.Translation += move;
}
view_ = Matrix.CreateLookAt(world_.Translation, world_.Forward + world_.Translation, world_.Up);
switch (Mode)
{
case CameraMode.PROJECTIVE:
projection_ = Matrix.CreatePerspectiveFieldOfView(MathHelper.PiOver4, Game.GraphicsDevice.Viewport.AspectRatio, 0.1f, 100);
break;
case CameraMode.ORTHOGONAL:
projection_ = Matrix.CreateOrthographic(Game.GraphicsDevice.Viewport.Width/20, Game.GraphicsDevice.Viewport.Height/20, 0.1f, 100);
break;
}
base.Update(gameTime);
}
public static float AngleBetween(this Vector3 v1, Vector3 v2)
{
float dot = Vector3.Dot(v1, v2);
float mag1 = Math.Abs(v1.Length());
float mag2 = Math.Abs(v2.Length());
return (float)Math.Acos(dot / (mag1 * mag2));
}
/// <summary>
/// Constructs a new GoalPoint.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="orientation">The orientation.</param>
/// <param name="scale">The amount to scale by.</param>
/// <param name="nextLevel">
/// The name of the next level to load.
/// An invalid name will return to the Main menu.
/// </param>
/// <param name="partType">The part required to pass the level.</param>
public GoalPoint(Vector3 position, Quaternion orientation, Vector3 scale, string nextLevel, Type partType)
: base(new ScrollingTransparentModel("tractorBeam", new Vector2(0.1f, 0.0f)), new Cylinder(position, 1f, scale.Length() * 7.0f))
{
mNextLevel = nextLevel;
PartType = partType;
Scale = new Vector3(1.0f, 5.0f, 1.0f);
}
/// <summary>
/// Truncates the vector in accordance to the maximal length
/// </summary>
/// <param name="vectorToTruncate">Vector to truncate</param>
/// <param name="maxLengthOfVector">Maximal legal length of the vector</param>
/// <returns>Truncated vector</returns>
public static Vector3 Truncate(Vector3 vectorToTruncate, float maxLengthOfVector)
{
if (vectorToTruncate.Length() > maxLengthOfVector)
vectorToTruncate = Vector3.Normalize(vectorToTruncate) * maxLengthOfVector;
return vectorToTruncate;
}
/// <summary>
/// Moves the constraint lines to the proper location relative to the entities involved.
/// </summary>
public override void Update()
{
//Move lines around
axis.PositionA = MathConverter.Convert(LineObject.ConnectionB.Position);
axis.PositionB = MathConverter.Convert(LineObject.ConnectionB.Position + LineObject.TwistAxisB * 1.5f);
float angleIncrement = 4 * MathHelper.Pi / limitLines.Length; //Each loop iteration moves this many radians forward.
for (int i = 0; i < limitLines.Length / 2; i++)
{
Line pointToPreviousPoint = limitLines[2 * i];
Line centerToPoint = limitLines[2 * i + 1];
float currentAngle = i * angleIncrement;
//Using the parametric equation for an ellipse, compute the axis of rotation and angle.
Microsoft.Xna.Framework.Vector3 rotationAxis = MathConverter.Convert(LineObject.Basis.xAxis * LineObject.MaximumAngleX * (float)Math.Cos(currentAngle) +
LineObject.Basis.yAxis * LineObject.MaximumAngleY * (float)Math.Sin(currentAngle));
float angle = rotationAxis.Length();
rotationAxis /= angle;
pointToPreviousPoint.PositionA = MathConverter.Convert(LineObject.ConnectionB.Position) +
//Rotate the primary axis to the ellipse boundary...
Microsoft.Xna.Framework.Vector3.TransformNormal(MathConverter.Convert(LineObject.Basis.PrimaryAxis), Microsoft.Xna.Framework.Matrix.CreateFromAxisAngle(rotationAxis, angle));
centerToPoint.PositionA = pointToPreviousPoint.PositionA;
centerToPoint.PositionB = MathConverter.Convert(LineObject.ConnectionB.Position);
}
for (int i = 0; i < limitLines.Length / 2; i++)
{
//Connect all the pointToPreviousPoint lines to the previous points.
limitLines[2 * i].PositionB = limitLines[2 * ((i + 1) % (limitLines.Length / 2))].PositionA;
}
}
public Vector3 pursue(Vector3 evaderPosition, Vector3 evaderVelocity,Vector3 currentPosition,Vector3 velocity)
{
float distance = Vector3.Subtract(evaderPosition, currentPosition).Length();
float timeToReachTarget = distance / velocity.Length();
Vector3 targetPosition = evaderPosition + evaderVelocity * timeToReachTarget;
return seek(targetPosition,currentPosition,velocity);
}
public void Update(float dt)
{
Position += Velocity * dt;
Velocity -= new Vector3(0, Gravity * dt, 0);
Velocity *= (1f - Damping);
Vector3 direction = Velocity;
direction.Normalize();
Ray ray = new Ray(Position, direction);//Vector3.Transform(Vector3.Forward, Orientation));
RayCastResult result;
Sector.Redria.Space.RayCast(ray, 2, out result);
if (result.HitObject != null)
{
if (result.HitObject.Tag is ShipObj)
{
var ship = (ShipObj)result.HitObject.Tag;
var magnitude = Velocity.Length();
//var angle = Vector3.Dot(result.HitData.Normal,Velocity)/magnitude;
float velocity = magnitude;//(float)(magnitude*angle);
ship.BulletStrike(Data,velocity);
}
Sector.Redria.Bullets.Remove(this);
}
if (Position.Y < -5) Sector.Redria.Bullets.Remove(this);
}
public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)
{
predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;
//exponential map
Vector3 axis;
float angle = angularVelocity.Length();
//limit the angular motion
if (angle * timeStep > AngularMotionTreshold)
{
angle = AngularMotionTreshold / timeStep;
}
if (angle < 0.001f)
{
// use Taylor's expansions of sync function
axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);
}
Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));
Quaternion ornA = MatrixOperations.GetRotation(currentTransform);
Quaternion predictedOrn = dorn * ornA;
predictedOrn.Normalize();
MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);
Matrix test = Matrix.CreateFromQuaternion(predictedOrn);
}
public void ApplyForce(SimObject simObject)
{
//get the direction vector
direction = simObjectA.CurrPosition - simObjectB.CurrPosition;
//check for zero vector
if (direction != Vector3.Zero)
{
//get length
currLength = direction.Length();
//normalize
direction.Normalize();
//add spring force
force = -stiffness * ((currLength - restLength) * direction);
//add spring damping force
force += -damping * Vector3.Dot(simObjectA.CurrVelocity - simObjectB.CurrVelocity, direction) * direction;
//apply the equal and opposite forces to the objects
simObjectA.ResultantForce += force;
simObjectB.ResultantForce += -force;
}
}
public ConstantLinearMoveBehavior(Vector3 v)
: base()
{
Speed = v.Length();
v.Normalize();
Direction = v;
}
public static Vector3 dotProductCalculation(Vector3 direction1, Vector3 direction2)
{
if (direction1.Length() == 0 || direction2.Length() == 0)
{
return Vector3.Zero;
}
else
{
Vector3 normalized1 = new Vector3(direction1.X, direction1.Y, direction1.Z);
normalized1.Normalize();
Vector3 normalized2 = new Vector3(direction2.X, direction2.Y, direction2.Z);
normalized2.Normalize();
float dotProduct = Vector3.Dot(normalized1, normalized2);
return dotProduct * normalized2 * direction1.Length();
}
}
/// <summary>
/// Construct this transform from a matrix
/// </summary>
/// <param name="matrix"></param>
/// <param name="performValidityCheck">When true, the matrix will be checked to make sure it will produce a valid transform</param>
public Transform(ref Matrix matrix, bool performValidityCheck)
{
if (performValidityCheck)
{
//validate the matrix is not sheered or inverted on a single axis.
Vector3 x = new Vector3(matrix.M11, matrix.M12, matrix.M13);
Vector3 y = new Vector3(matrix.M21, matrix.M22, matrix.M23);
Vector3 z = new Vector3(matrix.M31, matrix.M32, matrix.M33);
float xl = x.Length();
float yl = y.Length();
float zl = z.Length();
float maxl = Math.Max(xl, Math.Max(yl, zl));
float minl = Math.Min(xl, Math.Min(yl, zl));
if ((maxl - minl) > maxl / 10)
throw new ArgumentException("The input matrix is not uniformly scaled");
if (xl > 0.000001f) x /= xl;
if (yl > 0.000001f) y /= yl;
if (zl > 0.000001f) z /= zl;
Vector3 zc = Vector3.Cross(x, y);
Vector3 yc = Vector3.Cross(z, x);
Vector3 xc = Vector3.Cross(y, zc);
if (Vector3.Dot(x, xc) < 0.975f || Vector3.Dot(z, zc) * Vector3.Dot(y, yc) < 0.95f)
throw new ArgumentException("The input matrix is skewed, sheered or non uniformly scaled");
}
Vector3 scale;
matrix.Decompose(out scale, out Rotation, out Translation);
//if one or two components are negative, then the Decompose messed up.
if (scale.X * scale.Y * scale.Z < 0)
{
Matrix copy = matrix;
copy.M11 = -copy.M11;
copy.M12 = -copy.M12;
copy.M13 = -copy.M13;
copy.M21 = -copy.M21;
copy.M22 = -copy.M22;
copy.M23 = -copy.M23;
copy.M31 = -copy.M31;
copy.M32 = -copy.M32;
copy.M33 = -copy.M33;
copy.Decompose(out scale, out Rotation, out Translation);
scale = -scale;
}
this.Scale = Math.Min(Math.Min(scale.X, scale.Y), scale.Z);
if (Scale > 0.999f && Scale < 1.001f)
Scale = 1;
this.Rotation.Normalize();
}
public Light(Vector3 position, Vector3 gaze, Vector3 ambientColor, Vector3 diffuseColor, Vector3 specularColor)
{
mGaze = gaze;
Position = position;
mPositionPhi = (float)Math.Atan(Position.Y / Position.X);
mPositionTheta = (float)Math.Acos(Position.Z / Position.Length());
mAmbientColor = ambientColor;
mDiffuseColor = diffuseColor;
mSpecularColor = specularColor;
}
protected virtual void ExertFlapLeft(Vector3 leftHandVel)
{
Vector2 impulse = Vector2.UnitY * leftHandVel.Length() * FLAP_MULTIPLIER;
Matrix rot = Matrix.CreateRotationZ(ragdoll.Body.Rotation);
impulse = Vector2.Transform(impulse, rot);
ragdoll._upperLeftArm.ApplyLinearImpulse(impulse);
}
protected virtual bool ShouldFlap(Vector3 handLoc, Vector3 handVel)
{
if (handVel.Y < -1f && handLoc.Length() > .20f && handLoc.Y < 0)
{
//Console.WriteLine("HandVelY: " + handVel.Y + " handLocLength: " + handLoc.Length() + " handLocY: " + handLoc.Y);
return true;
}
return false;
}
public static RealtimeCSG.Plane TransformCopy(RealtimeCSG.Plane plane, Matrix by)
{
var lv = new Vector4(plane.A, plane.B, plane.C, plane.D);
lv = Vector4.Transform(lv, by);
var lv3 = new Vector3(lv.X, lv.Y, lv.Z);
lv.W *= lv3.Length();
lv3.Normalize();
lv.X = lv3.X;
lv.Y = lv3.Y;
lv.Z = lv3.Z;
return new RealtimeCSG.Plane(lv.X, lv.Y, lv.Z, lv.W);
}
/// <summary>
/// Returns the distance to the argument point as well as
/// the connectin Vector3 from the Point to this.
/// </summary>
/// <param name="point">The point to get the distance to.</param>
/// <param name="connectingVector">The connecting vector from the argument Pointn to this.</param>
/// <returns>The distance between this and the argument Point.</returns>
#endregion
public float DistanceTo(Point point, out Vector3 connectingVector)
{
connectingVector = new Vector3();
float segmentLength = (float)GetLength();
Vector2 normalizedLine = new Vector2(
(float)(Point2.X - Point1.X) / segmentLength,
(float)(Point2.Y - Point1.Y) / segmentLength);
Vector2 pointVector = new Vector2((float)(point.X - Point1.X), (float)(point.Y - Point1.Y));
float length = Vector2.Dot(pointVector, normalizedLine);
if (length < 0)
{
connectingVector.X = (float)(Point1.X - point.X);
connectingVector.Y = (float)(Point1.Y - point.Y);
connectingVector.Z = 0;
return(connectingVector.Length());
}
else if (length > segmentLength)
{
connectingVector.X = (float)(Point2.X - point.X);
connectingVector.Y = (float)(Point2.Y - point.Y);
connectingVector.Z = 0;
return(connectingVector.Length());
}
else
{
Point tempPoint = new Point(Point1.X + length * normalizedLine.X,
Point1.Y + length * normalizedLine.Y);
connectingVector.X = (float)(tempPoint.X - point.X);
connectingVector.Y = (float)(tempPoint.Y - point.Y);
connectingVector.Z = 0;
return(connectingVector.Length());
}
}
private float DistanceToPatch(Vector3 min, Vector3 max, float radius)
{
Vector3 mid1 = (min + max) / 2;
if (mid1 == Vector3.Zero)
{
mid1.Z = min.Z;
}
Vector3 surfaceMidPoint = Vector3.Transform(Vector3.Normalize(mid1) * radius, Transform.AbsoluteTransform);
return(surfaceMidPoint.Length());
}
public static bool IsSaneNormal(Vector3 v)
{
if (!Debug.Sanity(v))
{
return false;
}
if (!TrickyMath.AlmostEquals(v.Length(), 1f))
{
Report("Non normal normal");
return false;
}
return true;
}
public override void SweptTest(CollisionFunctor cf, Part partA, Part partB, Vector3 delta)
{
var a = (CapsulePart)partA;
var b = (CapsulePart)partB;
Vector3 step, offset = Vector3.Zero;
int steps = (int)(delta.Length() / a.World.Radius * 0.9f);
Vector3.Divide(ref delta, steps, out step);
while (steps-- >= 0 && !DoOverlapTest(cf, a, b, offset))
{
Vector3.Add(ref offset, ref step, out offset);
}
}
private float DistanceToPatch(Vector3 min, Vector3 max, float radius)
{
Vector3 mid1 = (min + max) / 2;
if (mid1 == Vector3.Zero)
{
mid1.Z = min.Z;
}
Vector3 surfaceMidPoint = Vector3.Transform(Vector3.Normalize(mid1) * radius, Transform.AbsoluteTransform);
Vector3 toMidPoint = SystemCore.ActiveCamera.Position - surfaceMidPoint;
return(toMidPoint.Length());
}
//Distance from ship
public bool Update(Vector3 Distance)
{
//Sets new position, makes the bullet move
if(move)
{
Position.Z -= 10;
Position.Z -= (float)(Math.Cos(Direction) * 200);
Position.X -= (float)(Math.Sin(Direction) * 200);
}
Distance -= Position;
return (Distance.Length()<20000);
}
public GameObject(Vector3 position = default (Vector3), Angles3 rotation = default (Angles3), Vector3 scale = default (Vector3),
bool isVisible = true, bool isSelectable = false, bool isMovable = false)
{
Position = position;
Scale = scale.Length () != 0 ? scale : Vector3.One;
IsVisible = isVisible;
IsSelectable = isSelectable;
IsMovable = isMovable;
Bounds = new BoundingSphere [0];
UniqueKey = GetType ().Name;
IsLightingEnabled = true;
IsSkyObject = false;
Coloring = new SingleColor (Color.Transparent);
}
//increase or decrease the length of vector by size
public static Vector3 AddVectorLength( Vector3 vector, float size )
{
//get the vector length
float magnitude = vector.Length();
//calculate new vector length
float newMagnitude = magnitude + size;
//calculate the ratio of the new length to the old length
float scale = newMagnitude / magnitude;
//scale the vector
return vector * scale;
}
/// <summary>Outputs the vertices for an arrow into the specified array</summary>
/// <param name="vertices">Array to write the arrow vertices into</param>
/// <param name="startIndex">Index in the array to begin writing at</param>
/// <param name="origin">
/// Location at which to draw the arrow (this will form the exact center of
/// the drawn arrow's base)
/// </param>
/// <param name="direction">
/// Direction the arrow is pointing into. The arrow's size is relative to
/// the length of this vector.
/// </param>
/// <param name="color">Color of the lines making up the arrow</param>
internal static void Generate(
VertexPositionColor[] vertices, int startIndex,
Vector3 origin, Vector3 direction, Color color
) {
// Build a vector pointing in an arbitrary direction that is perpendicular to
// the direction the arrow is pointing at. This can be done by simply juggling
// the vector elements around by one place.
Vector3 normalizedDirection = Vector3.Normalize(direction);
Vector3 up = VectorHelper.GetPerpendicularVector(normalizedDirection);
Vector3 right = Vector3.Cross(normalizedDirection, up);
float length = direction.Length();
up *= length;
right *= length;
Vector3 twoThird = origin + (direction * 0.667f);
Vector3 end = origin + direction;
// Line origin to arrowhead
vertices[startIndex].Position = origin;
vertices[startIndex].Color = color;
vertices[startIndex + 1].Position = end;
vertices[startIndex + 1].Color = color;
// Upper blade on arrowhead
vertices[startIndex + 2].Position = end;
vertices[startIndex + 2].Color = color;
vertices[startIndex + 3].Position = twoThird + up * 0.3f;
vertices[startIndex + 3].Color = color;
// Right blade on arrowhead
vertices[startIndex + 4].Position = end;
vertices[startIndex + 4].Color = color;
vertices[startIndex + 5].Position = twoThird + right * 0.3f;
vertices[startIndex + 5].Color = color;
// Lower blade on arrowhead
vertices[startIndex + 6].Position = end;
vertices[startIndex + 6].Color = color;
vertices[startIndex + 7].Position = twoThird + up * -0.3f;
vertices[startIndex + 7].Color = color;
// Left blade on arrowhead
vertices[startIndex + 8].Position = end;
vertices[startIndex + 8].Color = color;
vertices[startIndex + 9].Position = twoThird + right * -0.3f;
vertices[startIndex + 9].Color = color;
}
// solve unilateral constraint (equality, direct method)
public void ResolveUnilateralPairConstraint(
RigidBody body1, RigidBody body2,
Matrix world2A,
Matrix world2B,
Vector3 invInertiaADiag,
float invMassA,
Vector3 linvelA, Vector3 angvelA,
Vector3 rel_posA1,
Vector3 invInertiaBDiag,
float invMassB,
Vector3 linvelB, Vector3 angvelB,
Vector3 rel_posA2,
float depthA, Vector3 normalA,
Vector3 rel_posB1, Vector3 rel_posB2,
float depthB, Vector3 normalB,
out float imp0, out float imp1)
{
imp0 = 0;
imp1 = 0;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= float.Epsilon)
return;
BulletDebug.Assert(len < float.Epsilon);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * _tau * massTerm - vel0 * _damping;
float dv1 = depthB * _tau * massTerm - vel1 * _damping;
float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
}
public static float AngleWith(this Vector3 vector, Vector3 anotherVector, bool inRadiance = true)
{
const float closeTo1 = 1 - 1E-3f;
var normalizedDotProduct = Vector3.Dot(vector, anotherVector)/vector.Length()/anotherVector.Length();
if (normalizedDotProduct > closeTo1) //normalized dot product can have bad rounding errors
normalizedDotProduct = 1;
else if (normalizedDotProduct < -closeTo1)
normalizedDotProduct = -1;
var angleInRadiance = (float) Math.Acos(normalizedDotProduct);
if (inRadiance)
return angleInRadiance;
else
return MathHelper.ToDegrees(angleInRadiance);
}
public static RealtimeCSG.Plane[] CreatePrism(int sides, Vector3 radial, Vector3 axis, float height)
{
var result = new List<RealtimeCSG.Plane>();
result.Add(new RealtimeCSG.Plane(AsCsgVector(axis), height / 2));
result.Add(new RealtimeCSG.Plane(AsCsgVector(-axis), height / 2));
var radialLength = radial.Length();
for (int i = 0; i < sides; ++i)
{
var sideNormal = Vector3.Transform(radial, Matrix.CreateFromAxisAngle(axis, i * (Gem.Math.Angle.PI2 / sides)));
result.Add(new RealtimeCSG.Plane(AsCsgVector(Vector3.Normalize(sideNormal)), radialLength));
}
return result.ToArray();
}
public bool clicked(Vector3 victoriaPos, Vector3 clickVector,float yAdjust)
{
Vector3 circleVector = centerLocation - victoriaPos;
circleVector.Y *= yAdjust;
clickVector.Y *= yAdjust;
float dot = Vector3.Dot(circleVector, clickVector);
dot = dot / (circleVector.Length() * clickVector.Length());
if (dot < 0)
return false;
double angle = Math.Acos(dot);
double distance = Math.Sin(angle)*circleVector.Length();
if(distance>hitSphereRadius)
return false;
else
return true;
}
/// <summary>
/// Morph the object onto a spline. Expects the object's origin to already be aligned with the spline.
/// </summary>
/// <returns></returns>
public static void MorphToSpline(
Mesh mesh,
Vector3 axis,
Gem.Math.Spline spline)
{
Morph(mesh, (v) =>
{
var distance = Gem.Math.Vector.ProjectAOntoB(v, axis).Length() / axis.Length();
var splinePoint = spline.Point(distance);
var splineTangent = spline.Tangent(distance);
var rel = (axis * distance) - v;
var m = Matrix.CreateFromAxisAngle(Vector3.Normalize(spline.RotationAxis),//splineRotationAxis),
Gem.Math.Vector.AngleBetweenVectors(axis, splineTangent));
return splinePoint + Vector3.Transform(rel, m);
});
}
public Missile(ContentManager content, Vector3 moveVector, Vector3 position)
{
model = XNAUtils.LoadModelWithBoundingSphere(ref transforms, ResourceNames.Missile, content);
moveVector.Normalize();
Vector2 ZX = new Vector2(moveVector.Z, moveVector.X);
rotation = Quaternion.Identity;
float horizontalAngle = (float)Math.Asin(moveVector.X / ZX.Length());
if (moveVector.Z < 0)
horizontalAngle = MathHelper.Pi - horizontalAngle;
rotation *= Quaternion.CreateFromAxisAngle(Vector3.Up, horizontalAngle + MathHelper.PiOver2);
float verticalAngle = (float)Math.Asin(moveVector.Y / moveVector.Length());
rotation *= Quaternion.CreateFromAxisAngle(Vector3.Forward, verticalAngle);
this.scale = 0.1f;
this.moveVector = moveVector;
this.position = position;
}
/// <summary>
/// Determines whether there is a wall nearby.
/// </summary>
/// <param name="position">Position to look from.</param>
/// <param name="facingDirection">Direction to check in.</param>
/// <param name="filter">Anonymous function to filter out unwanted objects.</param>
/// <param name="space">The space to check for a wall in.</param>
/// <param name="distance">The distance to check within.</param>
/// <returns>True if a wall was detected, false otherwise.</returns>
public static bool FindWall(Vector3 position, Vector3 facingDirection, Func<BroadPhaseEntry, bool> filter, Space space, float distance, out RayCastResult result)
{
Ray forwardRay = new Ray(position, new Vector3(facingDirection.X, 0, facingDirection.Z));
result = new RayCastResult();
space.RayCast(forwardRay, filter, out result);
Vector3 flatNormal = new Vector3(result.HitData.Normal.X, 0, result.HitData.Normal.Z);
float normalDot = Vector3.Dot(result.HitData.Normal, flatNormal);
float minDot = (float)Math.Cos(MathHelper.PiOver4) * flatNormal.Length() * result.HitData.Normal.Length();
if ((result.HitData.Location - forwardRay.Position).Length() < distance && normalDot > minDot)
{
return true;
}
else
{
return false;
}
}
public void Update(GameTime gameTime)
{
Vector3 planetCenter = Transform.AbsoluteTransform.Translation;
Vector3 toCenterOfPlanet = Transform.AbsoluteTransform.Translation - SystemCore.ActiveCamera.Position;
float distanceToCenterOfPlanet = toCenterOfPlanet.Length();
float surfaceDistance = distanceToCenterOfPlanet - radius;
if (HasAtmosphere)
{
atmosphere.Update(sunDirection, SystemCore.ActiveCamera.Position, distanceToCenterOfPlanet);
atmosphericScatteringHelper.Update(distanceToCenterOfPlanet, sunDirection, SystemCore.ActiveCamera.Position - Transform.AbsoluteTransform.Translation);
}
float farPlaneMultiplier = MonoMathHelper.MapFloatRange(radius, radius * 2, 0.3f, 1f, surfaceDistance);
GenerateCustomProjectionMatrix(distanceToCenterOfPlanet * farPlaneMultiplier);
var frustrum = new BoundingFrustum(SystemCore.ActiveCamera.View * customProjection);
int activeCount = 0;
foreach (PlanetNode node in activePatches.Values)
{
node.Update();
//all nodes are flagged for removal every frame.
//The LOD calculation will unflag if nodes should be kept.
node.remove = true;
if (!frustrum.Intersects(node.boundingSphere))
{
node.Disable();
}
else
{
if (SystemWarGlobalSettings.RenderQuadtreeConnectivity)
{
RenderConnections(node);
}
node.Enable();
activeCount++;
}
}
if (SystemWarGlobalSettings.RepairSeams)
{
CalculateConnectivity();
}
for (int i = 0; i < rootNodes.Count; i++)
{
PlanetNode root = rootNodes[i];
CalculatePatchLOD(root.normal, root.step, root.depth, root.min, root.max);
}
//removes nodes that have not had their flags refreshed by the LOD pass
RemoveStaleNodes();
int patchCountPerFrame = 10;
for (int i = 0; i < patchCountPerFrame; i++)
{
PlanetNode finishedNode;
if (PlanetBuilder.GetBuiltNodes(Name, out finishedNode))
{
AddPatch(finishedNode);
}
}
}
