public void ApplyImpulse(float Delta)
{
if ((bodyA.Mass + bodyB.Mass) == 0)
{
InfiniteMassCorrection();
return;
}
for (int i = 0; i < Contacts.Count; i++)
{
vector2 rada = Contacts[i] - bodyA.Postition;
vector2 radb = Contacts[i] - bodyB.Postition;
vector2 rev = bodyB.Velocity + Vector2.Cross(bodyB.AngularVelocity, radb) - bodyB.Velocity - Vector2.Cross(bodyA.AngularVelocity, rada);
float ContactVelocity = Vector2.Dot(rev, Normal);
if (ContactVelocity > 0)
{
return;
}
///Apply Impulse
float radaCrossN = Vector2.Cross(rada, Normal);
float radbCrossN = Vector2.Cross(radb, Normal);
float InverseMassSum = bodyA.Mass + bodyB.Mass + (float)Math.Sqrt(radaCrossN) * bodyA.InverseInertia + (float)Math.Sqrt(radbCrossN) * bodyB.InverseInertia;
float ImpulseScaler = -(1 + CombinedRestitution) * ContactVelocity;
ImpulseScaler = (ImpulseScaler / InverseMassSum) / (float)Contacts.Count;
vector2 Impulse = Normal * ImpulseScaler;
bodyA.ApplyImpulse(-Impulse, rada);
bodyB.ApplyImpulse(Impulse, radb);
///Apply Friction Impulse
vector2 Tangent = rev - (Normal * Vector2.Dot(rev, Normal));
Tangent.Normalize();
float TangentScalar = -Vector2.Dot(rev, Tangent);
TangentScalar = (TangentScalar / InverseMassSum) / (float)Contacts.Count;
if (TangentScalar < 0.00f)
{
return;
}
vector2 TangentImpulse;
if (Math.Abs(TangentScalar) < ImpulseScaler * StaticFriction)
{
TangentImpulse = Tangent * TangentScalar;
}
else
{
TangentImpulse = Tangent * -ImpulseScaler * DynamicFriction;
}
bodyA.ApplyImpulse(-TangentImpulse, rada);
bodyB.ApplyImpulse(TangentImpulse, radb);
}
}
void CreatePoint2PointConstraint()
{
cubeRigidBody.LinearFactor = Vector3.Zero;
cubeRigidBody.AngularFactor = Vector3.Zero;
sphereRigidBody.LinearFactor = new Vector3(1, 1, 1);
sphereRigidBody.AngularFactor = new Vector3(1, 1, 1);
currentConstraint = Simulation.CreateConstraint(ConstraintTypes.Point2Point, cubeRigidBody, sphereRigidBody,
Matrix.Identity, Matrix.Translation(new Vector3(4, 0, 0)));
simulation.AddConstraint(currentConstraint);
constraintNameBlock.Text = "Point to Point";
//there are no limits so the sphere will orbit once we apply this
sphereRigidBody.ApplyImpulse(new Vector3(0, 0, 18));
}
public override void Start()
{
simulation = Entity.Get <PhysicsComponent>().Simulation;
simulation.Gravity = new Vector3(0, -9, 0);
cubeRigidBody = cube.Get <PhysicsComponent>()[0].RigidBody;
cubeRigidBody.CanSleep = false;
sphereRigidBody = sphere.Get <PhysicsComponent>()[0].RigidBody;
sphereRigidBody.CanSleep = false;
// Create the UI
constraintNameBlock = new TextBlock
{
Font = Font,
TextSize = 55,
TextColor = Color.White,
};
constraintNameBlock.SetCanvasPinOrigin(new Vector3(0.5f, 0.5f, 0));
constraintNameBlock.SetCanvasRelativePosition(new Vector3(0.5f, 0.93f, 0));
Entity.Get <UIComponent>().RootElement = new Canvas
{
Children =
{
constraintNameBlock,
CreateButton("Next Constraint",Font, 1),
CreateButton("Last Constraint",Font, -1)
}
};
// Create and initialize constraint
constraintsList.Add(CreatePoint2PointConstraint);
constraintsList.Add(CreateHingeConstraint);
constraintsList.Add(CreateGearConstraint);
constraintsList.Add(CreateSliderConstraint);
constraintsList.Add(CreateConeTwistConstraint);
constraintsList.Add(CreateGeneric6DoFConstraint);
constraintsList[constraintIndex]();
//Add a script for the slider constraint, to apply an impulse on collision
cubeRigidBody.ContactsAlwaysValid = true;
Script.AddTask(async() =>
{
while (Game.IsRunning)
{
var collision = await cubeRigidBody.NewCollision();
if (!(currentConstraint is SliderConstraint))
{
continue;
}
if (collision.ColliderA != sphereRigidBody && collision.ColliderB != sphereRigidBody)
{
continue;
}
sphereRigidBody.LinearVelocity = Vector3.Zero; //clear any existing velocity
sphereRigidBody.ApplyImpulse(new Vector3(-25, 0, 0)); //fire impulse
}
});
}
public void Impulse(Vector3 force)
{
if (force.LengthSquared() > float.Epsilon)
{
RigidBody.Activate(true);
RigidBody.ApplyImpulse(force.Cast(), BM.Vector3.Zero);
}
}
protected override void Apply(RigidBody obj, int slice)
{
Vector3D pos = this.FPosition[slice];
Vector3D force = this.FImpulse[slice];
obj.ApplyImpulse(new Vector3((float)force.x, (float)force.y, (float)force.z),
new Vector3((float)pos.x, (float)pos.y, (float)pos.z));
}
private void GoToMoving(float distToTarget, Vec2 toTarget)
{
if (distToTarget > 1)
{
MoveState = MoveStates.Moving;
if (RigidBody.GetLinearVelocity().Length() < _definition.MaxSpeed)
{
RigidBody.ApplyImpulse(toTarget * _definition.MoveImpulse, RigidBody.GetPosition());
}
}
}
public void CalculateImpactDistanceAndReact(Vector3 impactPos)
{
distanceToExplosion = (impactPos - rigidBody.CenterOfMassPosition).Length;
if (distanceToExplosion < 25)
{
var forceVector = rigidBody.CenterOfMassPosition - new Vector3(impactPos.X, impactPos.Y - 3, impactPos.Z);
forceVector.Normalize();
rigidBody.ApplyImpulse(forceVector * 1.75f, new Vector3(impactPos.X, impactPos.Y - 3, impactPos.Z));
}
}
protected override void Update(double deltaTime)
{
base.Update(deltaTime);
if (Input.IsKeyPressed(OpenTK.Input.Key.I))
{
rigidBody.ApplyImpulse(Vector3.UnitY * rand.Next(30, 100));
}
if (Input.IsKeyPressed(OpenTK.Input.Key.T))
{
rigidBody.ApplyTorqueImpulse(Vector3.UnitY * rand.Next(10, 50));
}
}
public static RigidBody crearBodyConImpulsoDoble(TGCVector3 origen, float radio, float masa, TGCVector3 director, float angulo)//este es para los disparos
{
RigidBody body = crearBodyEsferico(origen, radio, masa);
// var dir = director.ToBsVector;
//director.Normalize();
TGCVector3 dir = new TGCVector3(angulo, 0, 45);
//dir *= 50;
//body.LinearVelocity = dir * 75;
//body.LinearFactor = TGCVector3.One.ToBsVector;
body.ApplyImpulse(dir.ToBsVector, new TGCVector3(0, 20, 0).ToBsVector);//new TGCVector3(0, 15, 0).ToBsVector, new TGCVector3(0, 20, 0).ToBsVector);
return(body);
}
/// <summary>
/// Applies an impulse so that the velocity of point on the body is changed.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="positionWorld">The position on the body in world space.</param>
/// <param name="velocityWorld">The target velocity of the point in world space.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="body"/> is <see langword="null"/>.
/// </exception>
public static void SetVelocityOfWorldPoint(this RigidBody body, Vector3 positionWorld, Vector3 velocityWorld)
{
if (body == null)
{
throw new ArgumentNullException("body");
}
Vector3 oldVelocityWorld = body.GetVelocityOfWorldPoint(positionWorld);
var matrixK = ComputeKMatrix(body, positionWorld);
var constraintImpulse = matrixK.Inverse * ((velocityWorld - oldVelocityWorld));
body.ApplyImpulse(constraintImpulse, positionWorld);
}
/// <summary>
/// An impulse is applied an both contact points.
/// </summary>
/// <param name="impulse">The impulse to apply.</param>
public void ApplyImpulse(TSVector impulse)
{
#region INLINE - HighFrequency
//JVector temp;
if (!body1.isStatic)
{
body1.ApplyImpulse(-impulse, relativePos1);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(impulse, relativePos2);
}
#endregion
}
public override void Update()
{
Vector2 direction = currentTarget - Transform.Position;
direction = new Vector2(direction.X, 0f);
if ((Transform.WorldPosition2 - currentTarget).Length() > tolerance)
{
if (direction.X < 0)
{
Renderer.IsFlipped = true;
}
else
{
Renderer.IsFlipped = false;
}
direction.Normalize();
direction *= moveSpeed;
if (RigidBody.Body.LinearVelocity.LengthSquared() <= maxSpeed)
{
RigidBody.ApplyImpulse(direction);
}
Animation.Play("Run");
}
else
{
Animation.Play("Idle");
WaitTimeCounter += GameTimeGlobal.GameTime.ElapsedGameTime;
if (WaitTimeCounter > MaxWaitTime)
{
currentTarget = points[currentTargetCount];
currentTargetCount++;
if (currentTargetCount >= points.Count)
{
currentTargetCount = 0;
}
WaitTimeCounter = TimeSpan.Zero;
}
}
}
public void FixedUpdate(float timeStep)
{
animCtrl = animCtrl ?? GetComponent <AnimationController>();
body = body ?? GetComponent <RigidBody>();
// Update the in air timer. Reset if grounded
if (!onGround)
{
inAirTimer += timeStep;
}
else
{
inAirTimer = 0.0f;
}
// When character has been in air less than 1/10 second, it's still interpreted as being on ground
bool softGrounded = inAirTimer < CharacterDemo.InairThresholdTime;
// Update movement & animation
var rot = Node.Rotation;
Vector3 moveDir = Vector3.Zero;
var velocity = body.LinearVelocity;
// Velocity on the XZ plane
Vector3 planeVelocity = new Vector3(velocity.X, 0.0f, velocity.Z);
if (Controls.IsDown(CharacterDemo.CtrlForward))
{
moveDir += Vector3.UnitZ;
}
if (Controls.IsDown(CharacterDemo.CtrlBack))
{
moveDir += new Vector3(0f, 0f, -1f);
}
if (Controls.IsDown(CharacterDemo.CtrlLeft))
{
moveDir += new Vector3(-1f, 0f, 0f);
}
if (Controls.IsDown(CharacterDemo.CtrlRight))
{
moveDir += Vector3.UnitX;
}
// Normalize move vector so that diagonal strafing is not faster
if (moveDir.LengthSquared > 0.0f)
{
moveDir.Normalize();
}
// If in air, allow control, but slower than when on ground
body.ApplyImpulse(rot * moveDir * (softGrounded ? CharacterDemo.MoveForce : CharacterDemo.InairMoveForce));
if (softGrounded)
{
// When on ground, apply a braking force to limit maximum ground velocity
Vector3 brakeForce = -planeVelocity * CharacterDemo.BrakeForce;
body.ApplyImpulse(brakeForce);
// Jump. Must release jump control inbetween jumps
if (Controls.IsDown(CharacterDemo.CtrlJump))
{
if (okToJump)
{
body.ApplyImpulse(Vector3.UnitY * CharacterDemo.JumpForce);
okToJump = false;
}
}
else
{
okToJump = true;
}
}
// Play walk animation if moving on ground, otherwise fade it out
if (softGrounded && !moveDir.Equals(Vector3.Zero))
{
animCtrl.PlayExclusive("Models/Jack_Walk.ani", 0, true, 0.2f);
}
else
{
animCtrl.Stop("Models/Jack_Walk.ani", 0.2f);
}
// Set walk animation speed proportional to velocity
animCtrl.SetSpeed("Models/Jack_Walk.ani", planeVelocity.Length * 0.3f);
// Reset grounded flag for next frame
onGround = false;
}
public override void Update()
{
Vector2 movement = Vector2.Zero;
bool gpConnected = Input.GamepadState.IsConnected;
if (!isInVents)
{
bool canStealth = state == PlayerState.Crouching || state == PlayerState.Stealthing;
if (Input.KeyboardState.IsKeyDown(Keys.D) || gpConnected && Input.GamepadState.ThumbSticks.Left.X > 0)
{
movement.X += 1f;
Renderer.IsFlipped = true;
if (canStealth)
{
Animation.Play("Stealth");
state = PlayerState.Stealthing;
}
else
{
Animation.Play("Run");
state = PlayerState.Walking;
}
}
if (Input.KeyboardState.IsKeyDown(Keys.A) || gpConnected && Input.GamepadState.ThumbSticks.Left.X < 0)
{
movement.X -= 1f;
Renderer.IsFlipped = false;
if (canStealth)
{
Animation.Play("Stealth");
state = PlayerState.Stealthing;
}
else
{
Animation.Play("Run");
state = PlayerState.Walking;
}
}
if (movement.Length() > 0)
{
movement.Normalize();
movement /= RigidBody.MInPx;
if (RigidBody.Body.LinearVelocity.LengthSquared() < maxSpeed)
{
RigidBody.ApplyImpulse(movement * moveSpeed);
}
return;
}
state = PlayerState.Idle;
if (Input.KeyboardState.IsKeyDown(Keys.LeftControl) || gpConnected && Input.GamepadState.IsButtonDown(Buttons.Y))
{
Animation.Play("Hide");
state = PlayerState.Crouching;
}
if (hasShank && Input.KeyboardState.IsKeyDown(Keys.F) || gpConnected && Input.GamepadState.IsButtonDown(Buttons.X))
{
Animation.Play("Stab");
state = PlayerState.Attacking;
Audio.Play("Shank");
}
if (state == PlayerState.Idle)
{
Animation.Play("Idle");
}
}
// In vents movement
else
{
if (Input.KeyboardState.IsKeyDown(Keys.D) || gpConnected && Input.GamepadState.ThumbSticks.Left.X > 0)
{
movement.X += 1f;
Animation.Play("VentCrawl");
}
if (Input.KeyboardState.IsKeyDown(Keys.A) || gpConnected && Input.GamepadState.ThumbSticks.Left.X < 0)
{
movement.X -= 1f;
Animation.Play("VentCrawl");
}
if (Input.KeyboardState.IsKeyDown(Keys.W) || gpConnected && Input.GamepadState.ThumbSticks.Left.Y > 0)
{
movement.Y += 1f;
Animation.Play("VentCrawl");
}
if (Input.KeyboardState.IsKeyDown(Keys.S) || gpConnected && Input.GamepadState.ThumbSticks.Left.Y < 0)
{
movement.Y -= 1f;
Animation.Play("VentCrawl");
}
if (movement.Length() > 0)
{
movement.Normalize();
movement /= RigidBody.MInPx;
if (RigidBody.Body.LinearVelocity.LengthSquared() < maxSpeed)
{
RigidBody.ApplyImpulse(movement * moveSpeed);
}
return;
}
Animation.Play("VentIdle");
}
}
public void FixedUpdate(float timeStep)
{
animCtrl = animCtrl ?? GetComponent<AnimationController>();
body = body ?? GetComponent<RigidBody>();
// Update the in air timer. Reset if grounded
if (!onGround)
inAirTimer += timeStep;
else
inAirTimer = 0.0f;
// When character has been in air less than 1/10 second, it's still interpreted as being on ground
bool softGrounded = inAirTimer < CharacterDemo.InairThresholdTime;
// Update movement & animation
var rot = Node.Rotation;
Vector3 moveDir = Vector3.Zero;
var velocity = body.LinearVelocity;
// Velocity on the XZ plane
Vector3 planeVelocity = new Vector3(velocity.X, 0.0f, velocity.Z);
if (Controls.IsDown(CharacterDemo.CtrlForward))
moveDir += Vector3.UnitZ;
if (Controls.IsDown(CharacterDemo.CtrlBack))
moveDir += new Vector3(0f, 0f, -1f);
if (Controls.IsDown(CharacterDemo.CtrlLeft))
moveDir += new Vector3(-1f, 0f, 0f);
if (Controls.IsDown(CharacterDemo.CtrlRight))
moveDir += Vector3.UnitX;
// Normalize move vector so that diagonal strafing is not faster
if (moveDir.LengthSquared > 0.0f)
moveDir.Normalize();
// If in air, allow control, but slower than when on ground
body.ApplyImpulse(rot * moveDir * (softGrounded ? CharacterDemo.MoveForce : CharacterDemo.InairMoveForce));
if (softGrounded)
{
// When on ground, apply a braking force to limit maximum ground velocity
Vector3 brakeForce = -planeVelocity * CharacterDemo.BrakeForce;
body.ApplyImpulse(brakeForce);
// Jump. Must release jump control inbetween jumps
if (Controls.IsDown(CharacterDemo.CtrlJump))
{
if (okToJump)
{
body.ApplyImpulse(Vector3.UnitY * CharacterDemo.JumpForce);
okToJump = false;
}
}
else
okToJump = true;
}
// Play walk animation if moving on ground, otherwise fade it out
if (softGrounded && !moveDir.Equals(Vector3.Zero))
animCtrl.PlayExclusive("Models/Jack_Walk.ani", 0, true, 0.2f);
else
animCtrl.Stop("Models/Jack_Walk.ani", 0.2f);
// Set walk animation speed proportional to velocity
animCtrl.SetSpeed("Models/Jack_Walk.ani", planeVelocity.Length * 0.3f);
// Reset grounded flag for next frame
onGround = false;
}
/// <summary>
/// Updates the RaycastRobot as a whole.
/// </summary>
/// <param name="step"></param>
public void UpdateVehicle(float step)
{
for (int i = 0; i < wheelInfo.Length; i++)
{
UpdateWheelTransform(i, false);
}
currentVehicleSpeedKmHour = 3.6f * RigidBody.LinearVelocity.Length;
Matrix chassisTrans = ChassisWorldTransform;
Vector3 forwardW = new Vector3(
chassisTrans[0, indexForwardAxis],
chassisTrans[1, indexForwardAxis],
chassisTrans[2, indexForwardAxis]);
if (Vector3.Dot(forwardW, RigidBody.LinearVelocity) < 0)
{
currentVehicleSpeedKmHour *= -1.0f;
}
// Simulate suspension
for (int i = 0; i < wheelInfo.Length; i++)
{
//float depth =
RayCast(wheelInfo[i]);
}
UpdateSuspension(step);
for (int i = 0; i < wheelInfo.Length; i++)
{
//apply suspension force
WheelInfo wheel = wheelInfo[i];
float suspensionForce = wheel.WheelsSuspensionForce;
if (suspensionForce > wheel.MaxSuspensionForce)
{
suspensionForce = wheel.MaxSuspensionForce;
}
Vector3 impulse = wheel.RaycastInfo.ContactNormalWS * suspensionForce * step;
Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition;
RigidBody.ApplyImpulse(impulse, relpos);
}
UpdateFriction(step);
for (int i = 0; i < wheelInfo.Length; i++)
{
WheelInfo wheel = wheelInfo[i];
Vector3 relpos = wheel.RaycastInfo.HardPointWS - RigidBody.CenterOfMassPosition;
Vector3 vel = RigidBody.GetVelocityInLocalPoint(relpos);
if (wheel.RaycastInfo.IsInContact)
{
Matrix chassisWorldTransform = ChassisWorldTransform;
Vector3 fwd = new Vector3(
chassisWorldTransform[0, indexForwardAxis],
chassisWorldTransform[1, indexForwardAxis],
chassisWorldTransform[2, indexForwardAxis]);
float proj = Vector3.Dot(fwd, wheel.RaycastInfo.ContactNormalWS);
fwd -= wheel.RaycastInfo.ContactNormalWS * proj;
float proj2;
Vector3.Dot(ref fwd, ref vel, out proj2);
wheel.DeltaRotation = (proj2 * step) / (wheel.WheelsRadius);
wheel.Rotation += wheel.DeltaRotation;
}
else
{
wheel.Rotation += wheel.DeltaRotation;
}
wheel.DeltaRotation *= 0.99f;//damping of rotation when not in contact
}
}
/// <summary>
/// update override
/// </summary>
/// <param name="gameTime"></param>
public override void Update(GameTime gameTime)
{
if (Active)
{
var rotationDegrees = RigidBody.GetAngle() * 180 / System.Math.PI;
if (Keyboard.GetState().IsKeyDown(Keys.OemOpenBrackets))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
//apply impulse to push the player to left
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees - 90);
RigidBody.ApplyImpulse(impulseVec * GameData.PlayerLateralImpulse
, RigidBody.GetPosition());
}
}
if (Keyboard.GetState().IsKeyDown(Keys.OemCloseBrackets))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
//apply impulse to push the player to right
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees + 90);
RigidBody.ApplyImpulse(impulseVec * GameData.PlayerLateralImpulse
, RigidBody.GetPosition());
}
}
if (FilteredInputListener.WasKeyPressed(Keys.Left))
{
WeaponInventory.SelectPreviousWeapon();
FilteredInputListener.ResetKey(Keys.Left);
}
if (FilteredInputListener.WasKeyPressed(Keys.Right))
{
WeaponInventory.SelectNextWeapon();
FilteredInputListener.ResetKey(Keys.Right);
}
if (Keyboard.GetState().IsKeyDown(Keys.W))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees);
if (Vec2.Dot(impulseVec, RigidBody.GetLinearVelocity()) == 0)
{
RigidBody.SetLinearVelocity(Vec2.Zero);
}
RigidBody.ApplyImpulse(impulseVec * GameData.PlayerImpulse
, RigidBody.GetPosition());
}
}
if (Keyboard.GetState().IsKeyDown(Keys.S))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees);
RigidBody.ApplyImpulse(impulseVec * -GameData.PlayerImpulse
, RigidBody.GetPosition());
}
}
if (Keyboard.GetState().IsKeyDown(Keys.D))
{
//DecreaseLinearVelocity(GameData.PlayerTurnVelocityDecrement, 1);
RigidBody.ApplyTorque(GameData.PlayerTurnTorque);
}
if (Keyboard.GetState().IsKeyDown(Keys.A))
{
//DecreaseLinearVelocity(GameData.PlayerTurnVelocityDecrement, 1);
RigidBody.ApplyTorque(-GameData.PlayerTurnTorque);
}
if (Keyboard.GetState().IsKeyDown(Keys.Space))
{
var weapon = WeaponInventory.GetSelectedWeapon();
if (weapon != null && weapon.RemainingAmmo > 0)
{
if (DateTime.Now - _lastProjectileTime > TimeSpan.FromMilliseconds(100))
{
ShootingEffect.Play();
_lastProjectileTime = DateTime.Now;
WeaponInventory.DecreaseAmmo(1);
SpawnProjectile(weapon.ProjectileName, ProjectileSource.Player);
}
}
}
if (Keyboard.GetState().IsKeyUp(Keys.W) &&
Keyboard.GetState().IsKeyUp(Keys.A) &&
Keyboard.GetState().IsKeyUp(Keys.S) &&
Keyboard.GetState().IsKeyUp(Keys.D))
{
DecreaseLinearVelocity(GameData.PlayerTurnVelocityDecrement, 0);
}
if (Keyboard.GetState().IsKeyUp(Keys.A) && Keyboard.GetState().IsKeyUp(Keys.D))
{
RigidBody.SetAngularVelocity(0);
}
}
}
/// <summary>
/// Updates the friction for the RaycastRobot.
/// </summary>
/// <param name="timeStep"></param>
public void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = NumWheels;
if (numWheel == 0)
{
return;
}
Array.Resize <Vector3>(ref forwardWS, numWheel);
Array.Resize <Vector3>(ref axle, numWheel);
Array.Resize <float>(ref forwardImpulse, numWheel);
Array.Resize <float>(ref sideImpulse, numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < NumWheels; i++)
{
RigidBody groundObject = wheelInfo[i].RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
sideImpulse[i] = 0;
forwardImpulse[i] = 0;
}
for (int i = 0; i < NumWheels; i++)
{
RobotWheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
Matrix wheelTrans = GetWheelTransformWS(i);
axle[i] = new Vector3(
wheelTrans[0, indexRightAxis],
wheelTrans[1, indexRightAxis],
wheelTrans[2, indexRightAxis]);
Vector3 surfNormalWS = wheel.RaycastInfo.ContactNormalWS;
float proj;
Vector3.Dot(ref axle[i], ref surfNormalWS, out proj);
axle[i] -= surfNormalWS * proj;
axle[i].Normalize();
Vector3.Cross(ref surfNormalWS, ref axle[i], out forwardWS[i]);
forwardWS[i].Normalize();
ResolveSingleBilateral(RootRigidBody, wheel.RaycastInfo.ContactPointWS,
groundObject, wheel.RaycastInfo.ContactPointWS,
0, axle[i], ref sideImpulse[i], timeStep);
sideImpulse[i] *= wheel.SlidingFriction;
}
else
{
if (wheel.Speed > 0)
{
wheel.Speed = Math.Max(wheel.Speed - wheel.FreeSpinDamping, 0f);
}
else if (wheel.Speed < 0)
{
wheel.Speed = Math.Min(wheel.Speed + wheel.FreeSpinDamping, 0f);
}
}
}
bool sliding = false;
for (int i = 0; i < NumWheels; i++)
{
RobotWheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
float rollingFriction = 0.0f;
if (groundObject != null)
{
Vector3 velocity = chassisBody.GetVelocityInLocalPoint(wheel.ChassisConnectionPointCS);
Vector3 localVelocity = Vector3.TransformNormal(velocity, Matrix.Invert(chassisBody.WorldTransform.Basis));
Vector3 forwardAxis = (UnityEngine.Quaternion.AngleAxis(90f, UnityEngine.Vector3.up) *
wheel.WheelAxleCS.ToUnity() / (MathUtil.SIMD_PI * wheel.WheelsRadius)).ToBullet();
float speed = Vector3.Dot(localVelocity, forwardAxis);
wheel.Speed = speed;
if (wheel.EngineForce != 0.0f)
{
//apply torque curves
float engineForce = wheel.EngineForce;
if (speed * engineForce > 0)
{
engineForce *= 1 - (Math.Abs(speed) / MaxWheelAngularVelocity);
}
rollingFriction = engineForce * timeStep;
if (!RootRigidBody.IsActive)
{
RootRigidBody.Activate();
}
}
else
{
float defaultRollingFrictionImpulse = 0.0f;
float maxImpulse = (wheel.Brake != 0) ? wheel.Brake : defaultRollingFrictionImpulse;
rollingFriction = CalcRollingFriction(RootRigidBody, groundObject, wheel.RaycastInfo.ContactPointWS, forwardWS[i], maxImpulse);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
forwardImpulse[i] = 0;
wheelInfo[i].SkidInfo = 1.0f;
if (groundObject != null)
{
wheelInfo[i].SkidInfo = 1.0f;
float maximp = wheel.WheelsSuspensionForce * timeStep * wheel.FrictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
forwardImpulse[i] = rollingFriction;
float x = forwardImpulse[i] * fwdFactor;
float y = sideImpulse[i] * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = maximp / (float)System.Math.Sqrt(impulseSquared);
wheelInfo[i].SkidInfo *= factor;
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < NumWheels; wheel++)
{
if (sideImpulse[wheel] != 0)
{
if (wheelInfo[wheel].SkidInfo < 1.0f)
{
forwardImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
sideImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
}
}
}
}
// apply the impulses
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
Vector3 rel_pos = wheel.RaycastInfo.ContactPointWS -
chassisBody.CenterOfMassPosition;
if (forwardImpulse[i] != 0)
{
chassisBody.ApplyImpulse(forwardWS[i] * forwardImpulse[i], rel_pos);
}
if (sideImpulse[i] != 0)
{
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
Vector3 rel_pos2 = wheel.RaycastInfo.ContactPointWS -
groundObject.CenterOfMassPosition;
Vector3 sideImp = axle[i] * sideImpulse[i];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
//Vector4 vChassisWorldUp = RigidBody.CenterOfMassTransform.get_Columns(indexUpAxis);
Vector3 vChassisWorldUp = new Vector3(
RigidBody.CenterOfMassTransform.Row1[indexUpAxis],
RigidBody.CenterOfMassTransform.Row2[indexUpAxis],
RigidBody.CenterOfMassTransform.Row3[indexUpAxis]);
float dot;
Vector3.Dot(ref vChassisWorldUp, ref rel_pos, out dot);
rel_pos -= vChassisWorldUp * (dot * (1.0f - wheel.RollInfluence));
#else
rel_pos[indexUpAxis] *= wheel.RollInfluence;
#endif
chassisBody.ApplyImpulse(sideImp, rel_pos);
//apply friction impulse on the ground
groundObject.ApplyImpulse(-sideImp, rel_pos2);
}
}
}
void PhysicsPreStep(float timeStep)
{
/// TODO: Could cache the components for faster access instead of finding them each frame
/// Also, switch to generic version of GetComponent
///
RigidBody body = (RigidBody)Node.GetComponent("RigidBody");
AnimationController animCtrl = (AnimationController)Node.GetComponent("AnimationController", true);
// Update the in air timer. Reset if grounded
if (!onGround)
{
inAirTimer += timeStep;
}
else
{
inAirTimer = 0.0f;
}
// When character has been in air less than 1/10 second, it's still interpreted as being on ground
bool softGrounded = inAirTimer < INAIR_THRESHOLD_TIME;
// Update movement & animation
Quaternion rot = Node.Rotation;
Vector3 moveDir = Vector3.Zero;
Vector3 velocity = body.LinearVelocity;
// Velocity on the XZ plane
Vector3 planeVelocity = new Vector3(velocity.X, 0.0f, velocity.Z);
var input = GetSubsystem <Input>();
if (input.GetKeyDown(Constants.KEY_W))
{
moveDir += Vector3.Forward;
}
if (input.GetKeyDown(Constants.KEY_S))
{
moveDir += Vector3.Back;
}
if (input.GetKeyDown(Constants.KEY_A))
{
moveDir += Vector3.Left;
}
if (input.GetKeyDown(Constants.KEY_D))
{
moveDir += Vector3.Right;
}
float breakAdjust = ((float)input.MouseMoveWheel) * timeStep * 4.0f;
breakForce -= breakAdjust;
breakForce = Clamp <float>(breakForce, MIN_BRAKE_FORCE, MAX_BRAKE_FORCE);
// Normalize move vector so that diagonal strafing is not faster
if (moveDir.LengthSquared > 0.0f)
{
moveDir.Normalize();
}
// If in air, allow control, but slower than when on ground
body.ApplyImpulse(rot * moveDir * (softGrounded ? MOVE_FORCE : INAIR_MOVE_FORCE));
if (softGrounded)
{
// When on ground, apply a braking force to limit maximum ground velocity
Vector3 brakeForce = -planeVelocity * breakForce;
body.ApplyImpulse(brakeForce);
// Jump. Must release jump control inbetween jumps
if (input.GetKeyDown(Constants.KEY_SPACE))
{
if (okToJump)
{
body.ApplyImpulse(Vector3.Up * JUMP_FORCE);
okToJump = false;
animCtrl.PlayExclusive("Models/Mutant/Mutant_Jump1.ani", 0, false, 0.2f);
}
}
else
{
okToJump = true;
}
}
if (!onGround)
{
animCtrl.PlayExclusive("Models/Mutant/Mutant_Jump1.ani", 0, false, 0.2f);
}
else
{
// Play walk animation if moving on ground, otherwise fade it out
if (softGrounded && !moveDir.Equals(Vector3.Zero))
{
animCtrl.PlayExclusive("Models/Mutant/Mutant_Run.ani", 0, true, 0.2f);
}
else
{
animCtrl.PlayExclusive("Models/Mutant/Mutant_Idle0.ani", 0, true, 0.2f);
}
// Set walk animation speed proportional to velocity
animCtrl.SetSpeed("Models/Mutant/Mutant_Run.ani", planeVelocity.Length * 0.3f);
}
// Reset grounded flag for next frame
onGround = false;
}
/// <summary>
/// Apply an impulse at a point. This immediately modifies the velocity. It wakes up the body.
/// </summary>
public override void ApplyLinearImpulse(Vector3D impulse)
{
jitterBody.ApplyImpulse(JitterDatatypesMapping.Convert(ref impulse));
}
private void ProcessInput(float delta)
{
// -------------------------------------------------------------------
// Walking
_dir = new Vector3();
Transform camXform = Camera.GlobalTransform;
Vector2 inputMovementVector = new Vector2();
if (Input.IsActionPressed("movement_forward"))
{
inputMovementVector.y += 1;
}
if (Input.IsActionPressed("movement_backward"))
{
inputMovementVector.y -= 1;
}
if (Input.IsActionPressed("movement_left"))
{
inputMovementVector.x -= 1;
}
if (Input.IsActionPressed("movement_right"))
{
inputMovementVector.x += 1;
}
inputMovementVector = inputMovementVector.Normalized();
// Basis vectors are already normalized.
_dir += -camXform.basis.z * inputMovementVector.y;
_dir += camXform.basis.x * inputMovementVector.x;
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Jumping
if (IsOnFloor() && Input.IsActionJustPressed("movement_jump"))
{
_vel.y = JumpSpeed;
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Sprinting
if (Input.IsActionPressed("movement_sprint"))
{
_isSprinting = true;
}
else
{
_isSprinting = false;
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Turning the flashlight on/off
if (Input.IsActionJustPressed("flashlight"))
{
if (_flashlight.IsVisibleInTree())
{
_flashlight.Hide();
}
else
{
_flashlight.Show();
}
}
// -------------------------------------------------------------------
// Changing _weapons
int _weaponChangeNumber = _weaponNameToNumber[_currentWeaponName];
if (Input.IsActionJustPressed("Weapon1"))
{
_weaponChangeNumber = 0;
}
if (Input.IsActionJustPressed("Weapon2"))
{
_weaponChangeNumber = 1;
}
if (Input.IsActionJustPressed("Weapon3"))
{
_weaponChangeNumber = 2;
}
if (Input.IsActionJustPressed("Weapon4"))
{
_weaponChangeNumber = 3;
}
if (Input.IsActionJustPressed("shift_weapon_positive"))
{
_weaponChangeNumber++;
}
if (Input.IsActionJustPressed("shift_weapon_negative"))
{
_weaponChangeNumber--;
}
_weaponChangeNumber = Mathf.Clamp(_weaponChangeNumber, 0, _weaponNumberToName.Count);
if (_weaponNumberToName[_weaponChangeNumber] != _currentWeaponName)
{
if (!_reloadingWeapon && !_changingWeapon)
{
_changingWeaponName = _weaponNumberToName[_weaponChangeNumber];
_changingWeapon = true;
_mouseScrollValue = _weaponChangeNumber;
}
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Firing Weapon
if (Input.IsActionPressed("fire") && !_changingWeapon && !_reloadingWeapon)
{
Weapon _currentWeapon = _weapons[_currentWeaponName];
if (_currentWeapon != null && _currentWeapon.AmmoInWeapon > 0)
{
if (AnimationPlayer.CurrentState == _currentWeapon.IdleAnimName)
{
AnimationPlayer.CallbackFunction = GD.FuncRef(this, nameof(FireBullet));
AnimationPlayer.SetAnimation(_currentWeapon.FireAnimName);
}
}
else
{
_reloadingWeapon = true;
}
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Reloading
if (!_reloadingWeapon && !_changingWeapon && Input.IsActionJustPressed("reload"))
{
Weapon _currentWeapon = _weapons[_currentWeaponName];
if (_currentWeapon != null && _currentWeapon.CanReload)
{
string _currentAnimState = AnimationPlayer.CurrentState;
bool _isReloading = false;
foreach (string _weapon in _weapons.Keys)
{
Weapon _weaponNode = _weapons[_weapon];
if (_weaponNode != null && _currentAnimState == _weaponNode.ReloadingAnimName)
{
_isReloading = true;
}
}
if (!_isReloading)
{
_reloadingWeapon = true;
}
}
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Changing and throwing grenades
if (Input.IsActionJustPressed("change_grenade"))
{
if (_currentGrenade == 0)
{
_currentGrenade = 1;
}
else if (_currentGrenade == 1)
{
_currentGrenade = 0;
}
}
if (Input.IsActionJustPressed("fire_grenade") && _grenadeAmmounts[_currentGrenade] > 0)
{
_grenadeAmmounts[_currentGrenade]--;
Grenade _grenadeClone;
if (_currentGrenade == 0)
{
_grenadeClone = (FragGrenade)_fragGrenadeScene.Instance();
}
else
{
_grenadeClone = (StickyGrenade)_stickyGrenadeScene.Instance();
// Sticky grenades will stick to the player if we do not pass ourselves
_grenadeClone.PlayerBody = this;
}
GetTree().Root.AddChild(_grenadeClone);
_grenadeClone.GlobalTransform = GetNode <Spatial>("Rotation_Helper/FragGrenade_Toss_Pos").GlobalTransform;
_grenadeClone.ApplyImpulse(new Vector3(0, 0, 0), _grenadeClone.GlobalTransform.basis.z * FragGrenadeThrowForce);
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Grabbing and throwing objects
if (Input.IsActionJustPressed("fire") && _currentWeaponName == "UNARMED")
{
if (_grabbedObject == null)
{
PhysicsDirectSpaceState _state = GetWorld().DirectSpaceState;
Vector2 _centerPosition = GetViewport().Size / 2;
Vector3 _rayFrom = Camera.ProjectRayOrigin(_centerPosition);
Vector3 _rayTo = _rayFrom + Camera.ProjectRayNormal(_centerPosition) * ObjectGrabRayDistance;
Godot.Collections.Array _exclude = new Godot.Collections.Array();
_exclude.Add(this);
_exclude.Add(GetNode <Area>("Rotation_Helper/Gun_Fire_Points/Knife/Area"));
Godot.Collections.Dictionary _rayResult = _state.IntersectRay(_rayFrom, _rayTo, _exclude);
if (_rayResult.Count != 0 && _rayResult["collider"] is RigidBody)
{
_grabbedObject = _rayResult["collider"];
RigidBody _grabbedRigid = (RigidBody)_grabbedObject;
_grabbedRigid.Mode = RigidBody.ModeEnum.Static;
_grabbedRigid.CollisionLayer = 0;
_grabbedRigid.CollisionMask = 0;
}
}
else
{
_grabbedRigid = (RigidBody)_grabbedObject;
_grabbedRigid.Mode = RigidBody.ModeEnum.Rigid;
_grabbedRigid.ApplyImpulse(new Vector3(0, 0, 0), -Camera.GlobalTransform.basis.z.Normalized() * ObjectThrowForce);
_grabbedRigid.CollisionLayer = 1;
_grabbedRigid.CollisionMask = 1;
_grabbedRigid = null;
_grabbedObject = null;
}
}
if (_grabbedObject != null)
{
_grabbedRigid = (RigidBody)_grabbedObject;
Transform _transform = new Transform(_grabbedRigid.GlobalTransform.basis, Camera.GlobalTransform.origin + (-Camera.GlobalTransform.basis.z.Normalized() * ObjectGrabDistance));
_grabbedRigid.GlobalTransform = _transform;
}
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Pause Popup
if (Input.IsActionJustPressed("ui_cancel"))
{
if (_pausePopup == null)
{
_pausePopup = (PausePopup)_pausePopupScene.Instance();
_pausePopup.Player = this;
_globals.CanvasLayer.AddChild(_pausePopup);
_pausePopup.PopupCentered();
Input.SetMouseMode(Input.MouseMode.Visible);
GetTree().Paused = true;
}
else
{
Input.SetMouseMode(Input.MouseMode.Captured);
_pausePopup = null;
}
}
// -------------------------------------------------------------------
}
public override void FixedUpdate(float timeStep)
{
//var timeStep = e.TimeStep;
if (rigidBody_ == null)
{
rigidBody_ = node_.GetComponent <RigidBody>(false);
}
var animCtrl = modelNode_.GetComponent <AnimationController>();
jumpTimer_ += timeStep;
crouchTimer_ += timeStep;
// Update the in air timer. Reset if grounded
if (!onGround_)
{
inAirTimer_ += timeStep;
}
else
{
inAirTimer_ = 0.0f;
}
// When character has been in air less than 1/10 second, it's still interpreted as being on ground
bool softGrounded = (inAirTimer_ < INAIR_THRESHOLD_TIME);
// Update movement & animation
var rot = Node.Rotation;
Vector3 moveDir = Vector3.Zero;
var velocity = rigidBody_.LinearVelocity;
// Velocity on the XZ plane
var planeVelocity = new Vector3(velocity.X, 0, velocity.Z);
float speed = planeVelocity.Length;
if (controls_.IsDown(CTRL_FORWARD))
{
moveDir += Vector3.Forward * 1.0f;
}
if (controls_.IsDown(CTRL_BACK))
{
moveDir += Vector3.Back * 0.6f;
}
if (controls_.IsDown(CTRL_LEFT))
{
moveDir += Vector3.Left * 0.75f;
}
if (controls_.IsDown(CTRL_RIGHT))
{
moveDir += Vector3.Right * 0.75f;
}
// Normalize move vector so that diagonal strafing is not faster
if (moveDir.LengthSquared > 1.0f)
{
moveDir.Normalize();
}
if (controls_.IsDown(CTRL_SHIFT))
{
moveDir *= 0.4f;
}
// Crouching
CollisionShape shape = node_.GetComponent <CollisionShape>();
Node dropDetectNode = Node.GetChild("DropDetect", true);
CollisionShape dropDetectShape = dropDetectNode.GetComponent <CollisionShape>();
Node standDetectNode = Node.GetChild("StandDetect", true);
CollisionShape standDetectShape = standDetectNode.GetComponent <CollisionShape>();
Node groundDetectNode = Node.GetChild("GroundDetect", true);
CollisionShape groundDetectShape = groundDetectNode.GetComponent <CollisionShape>();
bool crouch = ((controls_.IsDown(CTRL_CROUCH) && crouchTimer_ >= CROUCH_RECOVER) || !okToStand_);// || (!softGrounded && crouching_));
rigidBody_.DisableMassUpdate();
if (crouch)
{
if (softGrounded)
{
moveDir *= 0.3f;
}
dropDetectShape.Position = Lerp(dropDetectShape.Position, new Vector3(0.0f, 1.0f, 0.0f), timeStep * CROUCH_SPEED);
standDetectShape.Position = Lerp(standDetectShape.Position, new Vector3(0.0f, 2.1f, 0.0f), timeStep * CROUCH_SPEED);
groundDetectShape.Position = Lerp(groundDetectShape.Position, new Vector3(0.0f, 1.3f, 0.0f), timeStep * CROUCH_SPEED);
shape.Position = Lerp(shape.Position, new Vector3(0.0f, 1.5f, 0.0f), timeStep * CROUCH_SPEED);
shape.Size = Lerp(shape.Size, new Vector3(0.8f, 1.0f, 0.8f), timeStep * CROUCH_SPEED);
crouching_ = true;
}
else
{
dropDetectShape.Position = Lerp(dropDetectShape.Position, new Vector3(0.0f, 0.0f, 0.0f), timeStep * CROUCH_SPEED);
standDetectShape.Position = Lerp(standDetectShape.Position, new Vector3(0.0f, 1.1f, 0.0f), timeStep * CROUCH_SPEED);
groundDetectShape.Position = Lerp(groundDetectShape.Position, new Vector3(0.0f, 0.3f, 0.0f), timeStep * CROUCH_SPEED);
Vector3 pos = shape.Position;
Vector3 newPos = Lerp(pos, new Vector3(0.0f, 1.0f, 0.0f), timeStep * CROUCH_SPEED);
// Adjust rigid body position to prevent penetration into the ground when standing
if (softGrounded && !dropDetected_)
{
rigidBody_.SetPosition((rigidBody_.Position - (newPos - pos) * 2.0f));
}
else if (nearGround_)
{
rigidBody_.SetPosition((rigidBody_.Position - (newPos - pos) * 3.0f));
}
shape.Position = (newPos);
shape.Size = Lerp(shape.Size, new Vector3(0.8f, 2.0f, 0.8f), timeStep * CROUCH_SPEED);
if (crouching_)
{
crouchTimer_ = 0.0f;
}
crouching_ = false;
}
rigidBody_.EnableMassUpdate();
// Prevent bounce bugs when colliding against corners, while still allowing movement up ramps
if (highImpulseDetected_ && nearGround_)
{
Vector3 vel = (rigidBody_.LinearVelocity);
rigidBody_.SetLinearVelocity(new Vector3(vel.X, Math.Min(vel.Y, 0.0f), vel.Z));
}
// If in air, allow control, but slower than when on ground
if (softGrounded && okToJump_)
{
float moveMag = (moveDir.Length);
if (moveMag > 0.0001)
{
moveMag_ = Lerp(moveMag_, 1.0f, timeStep * MOVE_ACCELERATION);
}
else
{
moveMag_ = Lerp(moveMag_, 0.0f, timeStep * MOVE_DECELERATION);
}
velocity_ = Lerp(velocity_, rot * moveDir * moveMag_ * MOVE_FORCE, timeStep * AGILITY);
rigidBody_.ApplyImpulse(velocity_);
Vector3 brakeForce = (planeVelocity * -BRAKE_FORCE);
rigidBody_.ApplyImpulse(brakeForce);
}
else
{
if (!(dropDetected_ && nearGround_))
{
velocity_ = Lerp(velocity_, rot * moveDir * moveMag_ * INAIR_MOVE_FORCE, timeStep * INAIR_AGILITY);
rigidBody_.ApplyImpulse(velocity_);
Vector3 brakeForce = (planeVelocity * -INAIR_BRAKE_FORCE);
rigidBody_.ApplyImpulse(brakeForce);
}
}
// Jumping
if (!controls_.IsDown(CTRL_JUMP))
{
jumpReleased_ = true;
}
if (!okToJump_ && jumpTimer_ > JUMP_RECOVER && onGround_ && softGrounded && jumpReleased_)
{
okToJump_ = true;
}
if (softGrounded && !highImpulseDetected_)
{
if (controls_.IsDown(CTRL_JUMP) && okToJump_)
{
Vector3 vel = (rigidBody_.LinearVelocity);
//URHO3D_LOGINFO("JUMP " + String(vel.y_));
rigidBody_.SetLinearVelocity(new Vector3(vel.X, Math.Max(vel.Y, 0.0f), vel.Z));
rigidBody_.ApplyImpulse(Vector3.Up * JUMP_FORCE);
okToJump_ = false;
jumpTimer_ = 0.0f;
jumpReleased_ = false;
}
}
// Keep the character close the ground / prevent launching from high speed movement
if ((okToJump_ && inAirTimer_ > 0.0f && inAirTimer_ <= INAIR_THRESHOLD_TIME && speed > 0.1f && !dropDetected_)
||
(inAirTimer_ > INAIR_THRESHOLD_TIME && !dropDetected_))
{
rigidBody_.ApplyImpulse(new Vector3(0.0f, -9.81f * speed, 0.0f));
}
// Prevent sliding while on slopes
rigidBody_.UseGravity = (!onGround_);
// Add artificial gravity after some time to pull to ground faster
if (inAirTimer_ > 0.1f && velocity_.Length > 0.1f)
{
rigidBody_.ApplyForce(new Vector3(0.0f, -9.81f * 2.0f, 0.0f));
}
// Play walk animation if moving on ground, otherwise fade it out
if (softGrounded && !moveDir.Equals(Vector3.Zero))
{
animCtrl.PlayExclusive("Models/Jack_Walk.ani", 0, true, 0.2f);
}
else
{
animCtrl.Stop("Models/Jack_Walk.ani", 0.2f);
}
// Set walk animation speed proportional to velocity
animCtrl.SetSpeed("Models/Jack_Walk.ani", speed / 9.0f);
// Reset grounded flag for next frame
onGround_ = false;
nearGround_ = false;
dropDetected_ = true;
okToStand_ = true;
highImpulseDetected_ = false;
modelNode_.Position = Lerp(modelNode_.Position,
rigidBody_.Position + rigidBody_.LinearVelocity * timeStep * 4.0f,
timeStep * (10.0f + rigidBody_.LinearVelocity.Length / 8.0f
));
modelNode_.Rotation = rot;
}
protected override void Update(double deltaTime)
{
var cam = MMW.MainCamera;
var d = cam.Transform.WorldPosition - cam.Target;
d.Y = 0.0f;
d.Normalize();
var dirZ = d;
var dirX = dirZ * Matrix3.CreateRotationY(MathHelper.PiOver2);
var deltaDir = Vector3.Zero;
if (Input.IsKeyDown(Key.W))
{
deltaDir -= dirZ;
}
if (Input.IsKeyDown(Key.S))
{
deltaDir += dirZ;
}
if (Input.IsKeyDown(Key.A))
{
deltaDir -= dirX;
}
if (Input.IsKeyDown(Key.D))
{
deltaDir += dirX;
}
if (deltaDir != Vector3.Zero)
{
deltaDir.Normalize();
var move = deltaDir * (float)deltaTime * 6.0f;
Velocity += move;
var length = Velocity.Length;
if (length > 2.5f)
{
length = 2.5f;
}
Velocity = Velocity.Normalized() * length;
Transform.Position += Velocity * (float)deltaTime;
//rb.ApplyForce(deltaDir * (float)deltaTime * 12.5f * rb.Mass * 50.0f);
userData.TotalMoveDistance += (Velocity * (float)deltaTime).Length;
if (CameraType == "third person")
{
var dot = -(Vector3.Dot(Transform.WorldDirectionZ, deltaDir) - 1.0f) * 0.5f * MathHelper.Pi;
var cross = Vector3.Cross(Transform.WorldDirectionZ, deltaDir);
var r = MMWMath.Clamp(dot, 0.0f, (float)deltaTime * 8.0f);
if (cross.Y > 0.0f)
{
Transform.Rotate.Y -= r;
}
else
{
Transform.Rotate.Y += r;
}
}
}
else
{
var length = Velocity.Length;
length -= (float)deltaTime * 5.0f;
if (length <= 0.0f)
{
Velocity = Vector3.Zero;
}
else
{
Velocity = Velocity.Normalized() * length;
}
Transform.Position += Velocity * (float)deltaTime;
}
if (Input.IsKeyPressed(Key.Space))
{
var wp = Transform.WorldPosition;
var rays = Bullet.RayTest(wp + Vector3.UnitY * 0.1f, wp - Vector3.UnitY * 0.2f, GameObject);
if (rays.Count > 0)
{
rb.ApplyImpulse(Vector3.UnitY * 4.5f * rb.Mass);
userData.TotalJumpCount++;
}
}
Transform.UpdatePhysicalTransform();
}
// Solve impulse and apply
public void ApplyImpulse()
{
// Early out and positional correct if both objects have infinite mass
if (NearEquals(A.InverseMass + B.InverseMass, 0))
{
A.Velocity.Set(0, 0);
B.Velocity.Set(0, 0);
return;
}
for (var i = 0; i < Contact.Count; ++i)
{
// Calculate radii from COM to contact
var ra = Contact.GetPoint(i) - A.Position;
var rb = Contact.GetPoint(i) - B.Position;
// Relative velocity
var rv = B.Velocity + Cross(B.AngularVelocity, rb) -
A.Velocity - Cross(A.AngularVelocity, ra);
// Relative velocity along the normal
var contactVel = Dot(rv, Contact.Normal);
// Do not resolve if velocities are separating
if (contactVel > 0)
{
return;
}
var raCrossN = Cross(ra, Contact.Normal);
var rbCrossN = Cross(rb, Contact.Normal);
var invMassSum = A.InverseMass + B.InverseMass + (Pow(raCrossN, 2) * A.InverseIntertia) + (Pow(rbCrossN, 2) * B.InverseIntertia);
// Calculate impulse scalar
var j = -(1.0f + MixedRestitution) * contactVel;
j /= invMassSum;
j /= Contact.Count;
// Apply impulse
var impulse = Contact.Normal * j;
A.ApplyImpulse(-impulse, ra);
B.ApplyImpulse(impulse, rb);
// Friction impulse
rv = B.Velocity + Cross(B.AngularVelocity, rb) -
A.Velocity - Cross(A.AngularVelocity, ra);
var t = rv - (Contact.Normal * Dot(rv, Contact.Normal));
t.Normalize();
// j tangent magnitude
var jt = -Dot(rv, t);
jt /= Contact.Count;
jt /= invMassSum;
// Don't apply tiny friction impulses
if (NearEquals(jt, 0.0f))
{
return;
}
// Coulumb's law
Vector tangentImpulse;
if (Abs(jt) < j * MixedStaticFriction)
{
tangentImpulse = t * jt;
}
else
{
tangentImpulse = t * -j * MixedDynamicFriction;
}
// Apply friction impulse
A.ApplyImpulse(-tangentImpulse, ra);
B.ApplyImpulse(tangentImpulse, rb);
}
}
public void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = NumWheels;
if (numWheel == 0)
{
return;
}
Array.Resize(ref forwardWS, numWheel);
Array.Resize(ref axle, numWheel);
Array.Resize(ref forwardImpulse, numWheel);
Array.Resize(ref sideImpulse, numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < NumWheels; i++)
{
RigidBody groundObject = wheelInfo[i].RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
sideImpulse[i] = 0;
forwardImpulse[i] = 0;
}
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
Matrix wheelTrans = GetWheelTransformWS(i);
axle[i] = new Vector3(
wheelTrans[0, indexRightAxis],
wheelTrans[1, indexRightAxis],
wheelTrans[2, indexRightAxis]);
Vector3 surfNormalWS = wheel.RaycastInfo.ContactNormalWS;
float proj;
Vector3.Dot(ref axle[i], ref surfNormalWS, out proj);
axle[i] -= surfNormalWS * proj;
axle[i].Normalize();
Vector3.Cross(ref surfNormalWS, ref axle[i], out forwardWS[i]);
forwardWS[i].Normalize();
ResolveSingleBilateral(chassisBody, wheel.RaycastInfo.ContactPointWS,
groundObject, wheel.RaycastInfo.ContactPointWS,
0, axle[i], ref sideImpulse[i], timeStep);
sideImpulse[i] *= sideFrictionStiffness2;
}
}
const float sideFactor = 1.0f;
const float fwdFactor = 0.5f;
bool sliding = false;
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
float rollingFriction = 0.0f;
if (groundObject != null)
{
if (wheel.EngineForce != 0.0f)
{
rollingFriction = wheel.EngineForce * timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0.0f;
float maxImpulse = (wheel.Brake != 0) ? wheel.Brake : defaultRollingFrictionImpulse;
rollingFriction = CalcRollingFriction(chassisBody, groundObject, wheel.RaycastInfo.ContactPointWS, forwardWS[i], maxImpulse);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
forwardImpulse[i] = 0;
wheelInfo[i].SkidInfo = 1.0f;
if (groundObject != null)
{
wheelInfo[i].SkidInfo = 1.0f;
float maximp = wheel.WheelsSuspensionForce * timeStep * wheel.FrictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
forwardImpulse[i] = rollingFriction;//wheel.EngineForce* timeStep;
float x = forwardImpulse[i] * fwdFactor;
float y = sideImpulse[i] * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = maximp / (float)Math.Sqrt(impulseSquared);
wheelInfo[i].SkidInfo *= factor;
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < NumWheels; wheel++)
{
if (sideImpulse[wheel] != 0)
{
if (wheelInfo[wheel].SkidInfo < 1.0f)
{
forwardImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
sideImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
}
}
}
}
// apply the impulses
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
Vector3 rel_pos = wheel.RaycastInfo.ContactPointWS -
chassisBody.CenterOfMassPosition;
if (forwardImpulse[i] != 0)
{
chassisBody.ApplyImpulse(forwardWS[i] * forwardImpulse[i], rel_pos);
}
if (sideImpulse[i] != 0)
{
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
Vector3 rel_pos2 = wheel.RaycastInfo.ContactPointWS -
groundObject.CenterOfMassPosition;
Vector3 sideImp = axle[i] * sideImpulse[i];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
//Vector4 vChassisWorldUp = RigidBody.CenterOfMassTransform.get_Columns(indexUpAxis);
Vector3 vChassisWorldUp = new Vector3(
RigidBody.CenterOfMassTransform.Row1[indexUpAxis],
RigidBody.CenterOfMassTransform.Row2[indexUpAxis],
RigidBody.CenterOfMassTransform.Row3[indexUpAxis]);
float dot;
Vector3.Dot(ref vChassisWorldUp, ref rel_pos, out dot);
rel_pos -= vChassisWorldUp * (dot * (1.0f - wheel.RollInfluence));
#else
rel_pos[indexUpAxis] *= wheel.RollInfluence;
#endif
chassisBody.ApplyImpulse(sideImp, rel_pos);
//apply friction impulse on the ground
groundObject.ApplyImpulse(-sideImp, rel_pos2);
}
}
}
public override void Update(GameTime gameTime)
{
#region move
if (Move.X != 0 || Move.Z != 0)
{
if (!activeAnimations["Walk"].Playing)
{
activeAnimations["Walk"].Play();
}
activeAnimations["Walk"].TimeModifier = new Vector2(RigidBody.LinearVelocity.X, RigidBody.LinearVelocity.Z).Length();
Vector3 forward = Matrix.CreateRotationY(BodyRotation).Forward;
Vector2 d = new Vector2(Move.X, -Move.Z);
d.Normalize();
Vector2 a = Vector2.SmoothStep(new Vector2(forward.X, -forward.Z), d, (float)gameTime.ElapsedGameTime.TotalSeconds * 15f);
BodyRotation = (float)Math.Atan2(a.Y, a.X) - MathHelper.PiOver2;
}
else
{
activeAnimations["Walk"].Stop();
}
Vector3 delta = Move - Velocity;
delta.Y = 0;
delta *= .2f;
if (delta.LengthSquared() > 0.1f)
{
RigidBody.ApplyImpulse(new JVector(delta.X, 0, delta.Z) * RigidBody.Mass);
}
JVector norm;
float frac = 0;
RigidBody hit;
bool cast = area.Physics.CollisionSystem.Raycast(RigidBody.Position + (JVector.Down * Height * .5f), JVector.Down,
(RigidBody bd, JVector n, float d) => {
return(bd != RigidBody);
},
out hit, out norm, out frac);
onGround = cast && frac < .1f;
if (Move.Y > 0 && Velocity.Y < Move.Y)
{
if (onGround)
{
RigidBody.ApplyImpulse(new JVector(0, Move.Y, 0) * RigidBody.Mass);
}
else if (Position.Y < area.WaterHeight)
{
Cell cell = area.CellAt(Position);
if (cell != null && cell.isLake)
{
RigidBody.AddForce(new JVector(0, 2 * Move.Y * RigidBody.Mass, 0));
}
}
}
#endregion
#region animate
List <Pose> curPoses = new List <Pose>();
foreach (KeyValuePair <string, Animation> kf in activeAnimations)
{
kf.Value.Update((float)gameTime.ElapsedGameTime.TotalSeconds);
if (kf.Value.Playing && !animRemove.Contains(kf.Key))
{
curPoses.Add(kf.Value.getCurrentPose());
}
}
Matrix[] t = new Matrix[transforms.Length];
for (int i = 0; i < t.Length; i++)
{
t[i] = Matrix.Identity;
}
int[] weights = new int[transforms.Length];
foreach (Pose p in curPoses)
{
Matrix[] pt = p.getTransforms();
// go thru all transforms in this pose, add the ones that have more weight
for (int i = 0; i < t.Length; i++)
{
if (p.Weight >= weights[i] && p.UsesBone(i))
{
t[i] = pt[i];
weights[i] = p.Weight;
}
}
}
transforms = t;
// rotate head
transforms[2] = Matrix.CreateTranslation(0, -3f * .125f, 0) *
Matrix.CreateRotationX(MathHelper.Clamp(MathHelper.WrapAngle(Look.X), -MathHelper.PiOver2 * .75f, MathHelper.PiOver2 * .75f)) *
Matrix.CreateRotationY(MathHelper.Clamp(MathHelper.WrapAngle(Look.Y - BodyRotation), -MathHelper.PiOver2 * .8f, MathHelper.PiOver2 * .8f)) *
Matrix.CreateTranslation(0, 3f * .125f, 0);
foreach (string s in animRemove)
{
activeAnimations.Remove(s);
}
animRemove.Clear();
#endregion
base.Update(gameTime);
}
