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