public static Derivative Evaluate(State initial, Vector3 acceleration, float dt, Derivative d)
{
State state;
state.position = initial.position + d.dx * dt;
state.velocity = initial.velocity + d.dv * dt;
Derivative output;
output.dx = state.velocity;
output.dv = acceleration;
return output;
}
public static State GainsFromFrequencies(Vector3 frequency, Vector3 dampingRatio, float mass)
{
State gainsState = new State();
gainsState.position = mass * frequency * frequency;
gainsState.velocity = dampingRatio * 2 * mass * frequency;
return gainsState;
}
public static State PIDMotionPlan(State oldState, State goalState, Vector3 acceleration, State gainsState, float timeDT)
{
Vector3 pidAccel = acceleration - gainsState.velocity * (oldState.velocity - goalState.velocity) - gainsState.position * (oldState.position - goalState.position);
return Integrate(oldState, pidAccel, timeDT);
}
public static State Integrate(State oldState, Vector3 accel, float timeDT)
{
Derivative orig;
orig.dx = Vector3.Zero;
orig.dv = Vector3.Zero;
Derivative a = Evaluate(oldState, accel, 0, orig);
Derivative b = Evaluate(oldState, accel, timeDT * 0.5f, a);
Derivative c = Evaluate(oldState, accel, timeDT * 0.5f, b);
Derivative d = Evaluate(oldState, accel, timeDT, c);
Vector3 dxdt = 1.0f / 6.0f * (a.dx + 2.0f * (b.dx + c.dx) + d.dx);
Vector3 dvdt = 1.0f / 6.0f * (a.dv + 2.0f * (b.dv + c.dv) + d.dv);
State newState;
newState.position = oldState.position + dxdt * timeDT;
newState.velocity = oldState.velocity + dvdt * timeDT;
return newState;
}
public override void OnUpdate()
{
if (!released)
{
physicsState.position = this.Transformation.GetPosition();
physicsState.velocity = Vector3.Zero;
}
Vector3 acceleration = Vector3.Up * -9.82f; //Gravity!
State newState = PhysicsHelper.Integrate(physicsState, acceleration, Time.GameTime.ElapsedTime);
Vector3 collNormal = Vector3.Zero;
bool collision = CheckCollision(newState, out collNormal);
if (released && collision)
{
if(!exploded)
{
exploded = true;
int minCells = 3;
int maxCells = 7;
int val = (int)MathHelper.Lerp(minCells, maxCells, explosionMagnitude / EXPLOSION_MAX_MAGNITUDE);
scene.MainTerrain.CarveTerrainAtPoint(newState.position, val, -45);
BoundingBox damageBounds = scene.MainTerrain.GetWorldSpaceBoundsAtPoint(newState.position, val);
Vector3 impulseVec = Vector3.Reflect(newState.velocity, collNormal);
for (int i = 0; i < scene.Actors.Count; i++)
{
if (damageBounds.Contains(scene.Actors[i].GetBounds()) != ContainmentType.Disjoint)
{
scene.Actors[i].ApplyDamage(this, impulseVec);
}
}
//tracerEmitter.EmitOnce = true;
this.scene.Entities.Remove(this);
this.OnDestroy();
}
}
else
{
physicsState = newState;
//tracerEmitter.Transformation.SetPosition(physicsState.position);
//tracerEmitter.Transformation.SetRotation(this.Transformation.GetRotation());
}
this.Transformation.SetPosition(physicsState.position);
this.renderElement.Transform = new Matrix[] { this.Transformation.GetTransform() };
base.OnUpdate();
}
bool CheckCollision(State state, out Vector3 collNormal)
{
bool collision = scene.MainTerrain.IsCollision(state.position, out collNormal);
if (collision)
return true;
Ray r = new Ray(state.position, Vector3.Normalize(state.velocity));
int currActorIndex = 0;
float maxValTimestep = state.velocity.Length()*Time.GameTime.ElapsedTime;
while (!collision && currActorIndex < scene.Actors.Count)
{
if (scene.Actors[currActorIndex].GetTeam() != sender.GetTeam())
{
float? intersectValue = r.Intersects(scene.Actors[currActorIndex].GetBounds());
if (intersectValue.HasValue)
{
if (intersectValue.Value <= maxValTimestep)
{
return true;
}
}
}
currActorIndex++;
}
return false;
}
public override void OnUpdate()
{
Vector3 acceleration = Vector3.Zero; //Gravity!
State newState = PhysicsHelper.Integrate(physicsState, acceleration, Time.GameTime.ElapsedTime);
physicsState = newState;
tracerEmitter.Transformation.SetPosition(physicsState.position);
tracerEmitter.Transformation.SetRotation(this.Transformation.GetRotation());
if (boundingBox.Contains(scene.MainCamera.GetPosition()) != ContainmentType.Disjoint && !collected)
{
collected = true;
scene.Entities.Remove(tracerEmitter);
Console.WriteLine("Heath absorbed");
scene.MainPlayer.ApplyHealth(20.0f);
// increase player's health
}
base.OnUpdate();
}
