void Update()
{
if (angularVelocity == null)
{
angularVelocity = GetComponent <MouseDirectionController>().getAngularVelocity();
}
float angleChange = angularVelocity.AngleChangeVelocity;
if (angleChange > 0 && !directionLeft || angleChange < 0 && directionLeft)
{
directionLeft = !directionLeft;
played = false;
}
if (Mathf.Abs(angleChange) > minAngularVel)
{
if (!played)
{
played = true;
if (turn != null)
{
turn();
}
}
}
}
public void ATTClientAttributeValueEvent(object sender, Bluegiga.BLE.Events.ATTClient.AttributeValueEventArgs e)
{
// check for a new value from the connected peripheral's heart rate measurement attribute
if (e.connection == GetConnectionHandle() && e.atthandle == att_handle_measurement)
{
aX = (e.value[1] * 256 + e.value[2]) * accelerometerFSR / Math.Pow(2, 16);
aY = (e.value[3] * 256 + e.value[4]) * accelerometerFSR / Math.Pow(2, 16);
aZ = (e.value[5] * 256 + e.value[6]) * accelerometerFSR / Math.Pow(2, 16);
gX = (e.value[7] * 256 + e.value[8]) * gyroFSR / Math.Pow(2, 16);
gY = (e.value[9] * 256 + e.value[10]) * gyroFSR / Math.Pow(2, 16);
gZ = (e.value[11] * 256 + e.value[12]) * gyroFSR / Math.Pow(2, 16);
AccelerationValue = new Acceleration(aX, aY, aZ);
AngularvelocityValue = new AngularVelocity(gX, gY, gZ);
double x0 = aX * aX + aY * aY + aZ * aZ;
if (x0 - x1 > 290)
{
if (originalStep < 0)
{
originalStep = step;
}
currentStep = step - originalStep;
PedometerValue = new PedometerData(step, calorie, distance, duration, currentStep);
step = step + 1;
}
x1 = x0;
}
}
public void Deinit()
{
// Environment
StaticAirTemperature.Remove();
StaticPressure.Remove();
// Orbit
Apoapsis.Remove();
Periapsis.Remove();
TimeToApoapsis.Remove();
// Vessel
Altitude.Remove();
AngularVelocity.Remove();
AvailablePosTorque.Remove();
AvailableRCSForce.Remove();
AvailableThrust.Remove();
AvailableTorque.Remove();
Direction.Remove();
DryMass.Remove();
Forward.Remove();
Mass.Remove();
MaxVacuumThrust.Remove();
MomentOfInertia.Remove();
Position.Remove();
Right.Remove();
Thrust.Remove();
Up.Remove();
VacuumSpecificImpulse.Remove();
Velocity.Remove();
}
public virtual void Update(GameTime gameTime)
{
float dT = (float)gameTime.ElapsedGameTime.TotalSeconds;
Position += Velocity * dT;
float l = AngularVelocity.Length();
if (l > 0.001f)
{
Quaternion deltaRot = new Quaternion();
Vector3 theta = AngularVelocity * dT;
float thetaMagSq = theta.LengthSquared();
float s;
if (thetaMagSq * thetaMagSq / 24.0f < 1e-6f)
{
deltaRot.W = 1.0f - thetaMagSq / 2.0f;
s = 1.0f - thetaMagSq / 6.0f;
}
else
{
float thetaMag = (float)Math.Sqrt(thetaMagSq);
deltaRot.W = (float)Math.Cos(thetaMag);
s = (float)Math.Sin(thetaMag) / thetaMag;
}
deltaRot.X = theta.X * s;
deltaRot.Y = theta.Y * s;
deltaRot.Z = theta.Z * s;
Rotation = deltaRot * Rotation;
}
}
public override int GetHashCode()
{
int hash = 1;
if (TimeStamp != 0D)
{
hash ^= pbc::ProtobufEqualityComparers.BitwiseDoubleEqualityComparer.GetHashCode(TimeStamp);
}
if (position_ != null)
{
hash ^= Position.GetHashCode();
}
if (orientation_ != null)
{
hash ^= Orientation.GetHashCode();
}
if (linearVelocity_ != null)
{
hash ^= LinearVelocity.GetHashCode();
}
if (angularVelocity_ != null)
{
hash ^= AngularVelocity.GetHashCode();
}
if (VesselName.Length != 0)
{
hash ^= VesselName.GetHashCode();
}
if (_unknownFields != null)
{
hash ^= _unknownFields.GetHashCode();
}
return(hash);
}
public Rudder(Angle maxposition, AngularVelocity velocity, Angle minimumdeflection)
{
Position = Angle.Zero;
MaxPosition = maxposition;
_velocity = velocity;
_minimumdeflection = minimumdeflection;
}
public Vector3 GetPointVelocity(Vector3 point)
{
var p = point - GlobalTransform.origin;
var v = AngularVelocity.Cross(p);
v = ToGlobal(v);
v += LinearVelocity;
return(ToLocal(v));
}
internal void Update(Rigidbody rigidbody)
{
// No need to read Position or Rotation. They're write-only from the patch to the component.
Velocity.FromUnityVector3(_sceneRoot.InverseTransformDirection(rigidbody.velocity));
AngularVelocity.FromUnityVector3(_sceneRoot.InverseTransformDirection(rigidbody.angularVelocity));
Mass = rigidbody.mass;
DetectCollisions = rigidbody.detectCollisions;
CollisionDetectionMode = (MRECollisionDetectionMode)Enum.Parse(typeof(MRECollisionDetectionMode), rigidbody.collisionDetectionMode.ToString());
UseGravity = rigidbody.useGravity;
IsKinematic = rigidbody.isKinematic;
ConstraintFlags = (MRERigidBodyConstraints)Enum.Parse(typeof(MRERigidBodyConstraints), rigidbody.constraints.ToString());
}
public void SimulateAir(Controller input, float dt)
{
const float M = 180.0f; // mass
const float J = 10.5f; // moment of inertia
const float v_max = 2300.0f;
const float w_max = 5.5f;
const float boost_force = 178500.0f;
const float throttle_force = 12000.0f;
Vector3 g = new Vector3(0.0f, 0.0f, -651.47f);
Vector3 rpy = new Vector3(input.Roll, input.Pitch, input.Yaw);
// air control torque coefficients
Vector3 T = new Vector3(-400.0f, -130.0f, 95.0f);
// air damping torque coefficients
Vector3 H = new Vector3(
-50.0f, -30.0f * (1.0f - Math.Abs(input.Pitch)),
-20.0f * (1.0f - Math.Abs(input.Yaw)));
float thrust = 0.0f;
if (input.Boost && Boost > 0)
{
thrust = (boost_force + throttle_force);
Boost--;
}
else
{
thrust = input.Throttle * throttle_force;
}
Velocity += (g + (thrust / M) * Forward) * dt;
Position += Velocity * dt;
Vector3 w_local = Vector3.Transform(AngularVelocity, Quaternion.Inverse(Rotation));
Vector3 old_w = AngularVelocity;
AngularVelocity += Vector3.Transform(T * rpy + H * w_local, Rotation) * (dt / J);
Vector3 angleAxis = 0.5f * (AngularVelocity + old_w) * dt;
Quaternion R = Quaternion.CreateFromAxisAngle(Vector3.Normalize(angleAxis), angleAxis.Length());
Rotation = Quaternion.Multiply(R, Rotation);
// if the velocities exceed their maximum values, scale them back
Velocity /= Math.Max(1.0f, Velocity.Length() / v_max);
AngularVelocity /= Math.Max(1.0f, AngularVelocity.Length() / w_max);
}
/// <summary>
/// Begins decelerating the <see cref="Target"/> based on any opposing drag forces.
/// </summary>
/// <returns>An Enumerator to manage the running state of the Coroutine.</returns>
protected virtual IEnumerator BeginDeceleration()
{
while (!Velocity.ApproxEquals(Vector3.zero, NilVelocityTolerance) || !AngularVelocity.ApproxEquals(Vector3.zero, NilAngularVelocityTolerance))
{
Velocity = Vector3.Slerp(Velocity, Vector3.zero, Drag * Time.deltaTime);
AngularVelocity = Vector3.Slerp(AngularVelocity, Vector3.zero, AngularDrag * Time.deltaTime);
Target.transform.localRotation *= Quaternion.Euler(AngularVelocity);
Target.transform.localPosition += Velocity * Time.deltaTime;
yield return(null);
}
decelerationRoutine = null;
Velocity = Vector3.zero;
AngularVelocity = Vector3.zero;
}
public MyVector3 GetMomentumAtPoint(MyVector3 point)
{
MyVector3 momentum = new MyVector3(); //Create momentum
if (AngularVelocity.Length() > 0)
{
MyVector3 pointVelocity = Velocity + VectorMaths.VectorCrossProduct(AngularVelocity, Transformation.Translation - point); //Set point velocity to be velocity + the cross product of the angular velocity and translation - point
momentum = Mass * pointVelocity; //Set momentum to be mass * point velocity
}
else
{
momentum = Mass * Velocity; //set momentum to be mass * velcity
}
return(momentum); //Return momentum
}
public void MergeFrom(NavigationRequest other)
{
if (other == null)
{
return;
}
if (other.TimeStamp != 0D)
{
TimeStamp = other.TimeStamp;
}
if (other.position_ != null)
{
if (position_ == null)
{
Position = new global::Navigation.Vec3();
}
Position.MergeFrom(other.Position);
}
if (other.orientation_ != null)
{
if (orientation_ == null)
{
Orientation = new global::Navigation.Quaternion();
}
Orientation.MergeFrom(other.Orientation);
}
if (other.linearVelocity_ != null)
{
if (linearVelocity_ == null)
{
LinearVelocity = new global::Navigation.Vec3();
}
LinearVelocity.MergeFrom(other.LinearVelocity);
}
if (other.angularVelocity_ != null)
{
if (angularVelocity_ == null)
{
AngularVelocity = new global::Navigation.Vec3();
}
AngularVelocity.MergeFrom(other.AngularVelocity);
}
if (other.VesselName.Length != 0)
{
VesselName = other.VesselName;
}
_unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields);
}
internal void Update(GodotRigidBody rigidbody)
{
Velocity.FromGodotVector3(_sceneRoot.ToLocal(rigidbody.LinearVelocity));
AngularVelocity.FromGodotVector3(_sceneRoot.ToLocal(rigidbody.AngularVelocity));
// No need to read Position or Rotation. They're write-only from the patch to the component.
Mass = rigidbody.Mass;
DetectCollisions = !rigidbody.GetChild <CollisionShape>()?.Disabled ?? false;
CollisionDetectionMode = rigidbody.ContinuousCd switch
{
true => MRECollisionDetectionMode.Continuous,
false => MRECollisionDetectionMode.Discrete
};
UseGravity = !Mathf.IsZeroApprox(rigidbody.GravityScale);
ConstraintFlags = rigidbody.GetMRERigidBodyConstraints();
}
private void updateMovement(TimeSpan elapsedTime, float acceleration, float leftRight)
{
var t = (float)elapsedTime.NumericValue;
this.speed += new Acceleration(acceleration * 50) * elapsedTime;
this.speed *= Mathf.Pow(1e-3f, t);
this.turnSpeed += AngularAcceleration.FromRadians(leftRight * 5) * elapsedTime;
this.turnSpeed *= Mathf.Pow(1e-7f, t);
this.Direction += this.turnSpeed * (elapsedTime * this.speed.NumericValue);
this.Position += this.Direction * this.speed * elapsedTime;
this.distanceTraveled += this.speed * elapsedTime;
}
public World()
{
Mesh = new Mesh(PointRobertsMarina, Length.FromMeters(1000));
var current = new Current(Bearing.East, Velocity.FromMPH(2), this);
var rudder = new Rudder(Angle.FromDegrees(45), AngularVelocity.FromDegreesPerSecond(90), Angle.FromDegrees(1));
var boat = new Boat(PointRobertsMarina, Velocity.FromMPH(25), Bearing.West, rudder);
rudder.MoveTo(Angle.FromDegrees(22));
boat.Autopilot.WayPoint = new Waypoint(Something);
_fleetingthings.Add(current);
_fleetingthings.Add(boat);
}
/// <summary>
/// Updates the sleeping state. Is called in <see cref="UpdateVelocity"/>.
/// </summary>
/// <param name="deltaTime">The time step.</param>
private void UpdateSleeping(float deltaTime)
{
if (IsSleeping || !CanSleep)
{
return;
}
if (MotionType == MotionType.Dynamic)
{
if (LinearVelocity.LengthSquared() < Simulation.Settings.Sleeping.LinearVelocityThresholdSquared &&
AngularVelocity.LengthSquared() < Simulation.Settings.Sleeping.AngularVelocityThresholdSquared)
{
// Movement is below threshold. Increase counter.
_noMovementTime += deltaTime;
// Static bodies sleep immediately. Kinematic and dynamic bodies are handled here.
// Dynamic bodies can only sleep if their whole island is sleeping. (Note: When the island
// is processed the sleeping of the dynamic body is deferred if the island is awake.)
if (_noMovementTime > Simulation.Settings.Sleeping.TimeThreshold)
{
IsSleeping = true;
}
}
else
{
// Movement detected.
_noMovementTime = 0;
}
}
else
{
if (LinearVelocity.LengthSquared() < Numeric.EpsilonFSquared &&
AngularVelocity.LengthSquared() < Numeric.EpsilonFSquared)
{
// Kinematic bodies are set to sleep immediately!
IsSleeping = true;
_linearVelocity = Vector3.Zero;
_angularVelocity = Vector3.Zero;
_noMovementTime = float.PositiveInfinity;
}
else
{
// Movement detected.
_noMovementTime = 0;
}
}
}
public void gyUpdate(AngularVelocity gyData)
{
DateTime timeref = DateTime.Now;
float timedif = (float)timeref.Subtract(Timestamp).TotalSeconds;
Console.WriteLine(timedif);
if (PrevAC != null)
{
Xac += PrevAC.X * timedif;
Yac += PrevAC.Y * timedif;
Zac += PrevAC.Z * timedif;
}
Xgy += gyData.X * timedif;
Ygy += gyData.Y * timedif;
Zgy += gyData.Z * timedif;
Timestamp = timeref;
}
public void UpdateDeactivation(float timeStep)
{
if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation))
{
return;
}
if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) &&
(AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold))
{
DeactivationTime += timeStep;
}
else
{
DeactivationTime = 0;
ActivationState = ActivationState.Nothing;
}
}
/// <summary>
/// Applies damping to reduce speeds
/// </summary>=
public void ApplyDamping(float dt)
{
float ne = GetKineticEnergy() * m_OneOverMass;
float ld = (1.0f - m_LinearDamping);
float ad = (1.0f - m_AngularDamping);
if ((ne >= m_SleepEnergyThreshold || ne > m_LastKineticEnergy) || AngularVelocity.LengthSquared() > SleepAngularVelocityThreshold)
{
m_DeactivationTimer += dt;
m_DeactivationTimer = m_DeactivationTimer < GLOBAL_DEACTIVATION_TIMER ? m_DeactivationTimer : GLOBAL_DEACTIVATION_TIMER;
}
else
{
m_DeactivationTimer -= dt;
m_DeactivationTimer = m_DeactivationTimer < 0.0f ? 0.0f : m_DeactivationTimer;
/* Do we really need this, when default AD is set?
* //Extra damping
* float d = m_DeactivationTimer / GLOBAL_DEACTIVATION_TIMER;
* d = d < 0.01 ? 0.01f : d;
*
* //Damping
* float d2 = d*d;
*
* ld = ld * d2;
* ad = ad * d2;*/
}
m_Velocity = m_Velocity * (float)Math.Pow(ld, dt);
m_AngularVelocity = m_AngularVelocity * (float)Math.Pow(ad, dt);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(m_AngularVelocity);
m_LastKineticEnergy = ne;
if (m_AngularVelocity.LengthSquared() < (0.001f * 0.001f))
{
m_AngularVelocity = Vector3.Zero;
}
if (m_Velocity.LengthSquared() < (0.02f * 0.02f))
{
m_Velocity = Vector3.Zero;
}
}
public override int GetHashCode()
{
int hash = 1;
if (header_ != null)
{
hash ^= Header.GetHashCode();
}
if (MeasurementTime != 0D)
{
hash ^= MeasurementTime.GetHashCode();
}
if (Type != 0)
{
hash ^= Type.GetHashCode();
}
if (position_ != null)
{
hash ^= Position.GetHashCode();
}
if (eulerAngles_ != null)
{
hash ^= EulerAngles.GetHashCode();
}
if (linearVelocity_ != null)
{
hash ^= LinearVelocity.GetHashCode();
}
if (angularVelocity_ != null)
{
hash ^= AngularVelocity.GetHashCode();
}
if (linearAcceleration_ != null)
{
hash ^= LinearAcceleration.GetHashCode();
}
hash ^= positionCovariance_.GetHashCode();
hash ^= eulerAnglesCovariance_.GetHashCode();
hash ^= linearVelocityCovariance_.GetHashCode();
hash ^= angularVelocityCovariance_.GetHashCode();
hash ^= linearAccelerationCovariance_.GetHashCode();
return(hash);
}
static async Task RunAsync(string[] args)
{
var metawear = await ScanConnect.Connect(args[0]);
Console.WriteLine($"Configuring {args[0]}...");
var acc = metawear.GetModule <IAccelerometer>();
var gyro = metawear.GetModule <IGyroBmi160>();
acc.Configure(odr: 25f);
gyro.Configure(odr: OutputDataRate._25Hz);
MergedData mergedata = new MergedData();
await gyro.AngularVelocity.AddRouteAsync(source => source.Stream(data =>
{
AngularVelocity gyroData = data.Value <AngularVelocity>();
mergedata.gyUpdate(gyroData);
//Console.Clear();
Console.WriteLine(mergedata.ToString());
}));
await acc.Acceleration.AddRouteAsync(source => source.Stream(data =>
{
Acceleration accData = data.Value <Acceleration>();
mergedata.acUpdate(accData);
//Console.Clear();
Console.WriteLine(mergedata.ToString());
}));
//Console.WriteLine(array1.ToString());
//gyro.AngularVelocity.Start();
//acc.Acceleration.Start
gyro.Start();
acc.Start();
//TODO Synchronise gyro and acc to get matching data
await Task.Delay(15000);
gyro.Stop();
acc.Stop();
Console.WriteLine();
//Console.WriteLine(x1);
Console.ReadLine();
}
public override int GetHashCode()
{
int hash = 1;
if (position_ != null)
{
hash ^= Position.GetHashCode();
}
if (orientation_ != null)
{
hash ^= Orientation.GetHashCode();
}
if (linearVelocity_ != null)
{
hash ^= LinearVelocity.GetHashCode();
}
if (linearAcceleration_ != null)
{
hash ^= LinearAcceleration.GetHashCode();
}
if (angularVelocity_ != null)
{
hash ^= AngularVelocity.GetHashCode();
}
if (Heading != 0D)
{
hash ^= Heading.GetHashCode();
}
if (linearAccelerationVrf_ != null)
{
hash ^= LinearAccelerationVrf.GetHashCode();
}
if (angularVelocityVrf_ != null)
{
hash ^= AngularVelocityVrf.GetHashCode();
}
if (eulerAngles_ != null)
{
hash ^= EulerAngles.GetHashCode();
}
return(hash);
}
/// <inheritdoc />
public virtual void Process()
{
if (!canProcess)
{
return;
}
if (!Velocity.ApproxEquals(Vector3.zero, NilVelocityTolerance) || !AngularVelocity.ApproxEquals(Vector3.zero, NilAngularVelocityTolerance))
{
float deltaTime = Time.inFixedTimeStep ? Time.fixedDeltaTime : Time.deltaTime;
Velocity = Vector3.Lerp(Velocity, Vector3.zero, Drag * deltaTime);
AngularVelocity = Vector3.Lerp(AngularVelocity, Vector3.zero, AngularDrag * deltaTime);
Target.transform.localRotation *= Quaternion.Euler(AngularVelocity);
Target.transform.localPosition += Velocity * deltaTime;
}
else
{
Velocity = Vector3.zero;
AngularVelocity = Vector3.zero;
canProcess = false;
}
}
/// <summary>
/// Initializes a new instance of the <see cref="RobotStateEventArgs"/> class.
/// </summary>
/// <param name="e">The event.</param>
internal RobotStateEventArgs(ExternalInterface.Event e) : base(e)
{
var robotState = e.RobotState;
Acceleration = new Acceleration(robotState.Accel);
CarryingObjectId = robotState.CarryingObjectId;
CarryingObjectOnTopId = robotState.CarryingObjectOnTopId;
Gyro = new AngularVelocity(robotState.Gyro);
HeadAngleRad = robotState.HeadAngleRad;
HeadTrackingObjectId = robotState.HeadTrackingObjectId;
LastImageTimestamp = robotState.LastImageTimeStamp;
LeftWheelSpeedMmps = robotState.LeftWheelSpeedMmps;
LiftHeightMm = robotState.LiftHeightMm;
LocalizedToObjectId = robotState.LocalizedToObjectId;
Pose = new Pose(robotState.Pose);
PoseAngleRad = robotState.PoseAngleRad;
PosePitchRad = robotState.PosePitchRad;
Proximity = new ProximitySensorData(robotState.ProxData);
RightWheelSpeedMmps = robotState.RightWheelSpeedMmps;
Status = new RobotStatus(robotState.Status);
Touch = new TouchSensorData(robotState.TouchData);
}
public void ATTClientAttributeValueEvent(object sender, Bluegiga.BLE.Events.ATTClient.AttributeValueEventArgs e)
{
// check for a new value from the connected peripheral's heart rate measurement attribute
if (e.connection == GetConnectionHandle() && e.atthandle == att_handle_measurement)
{
aX = BitConverter.ToInt16(e.value, 0) * accelerometerFSR / Math.Pow(2, 16);
aY = BitConverter.ToInt16(e.value, 2) * accelerometerFSR / Math.Pow(2, 16);
aZ = BitConverter.ToInt16(e.value, 4) * accelerometerFSR / Math.Pow(2, 16);
gX = BitConverter.ToInt16(e.value, 6) * gyroFSR / Math.Pow(2, 16);
gY = BitConverter.ToInt16(e.value, 8) * gyroFSR / Math.Pow(2, 16);
gZ = BitConverter.ToInt16(e.value, 10) * gyroFSR / Math.Pow(2, 16);
AccelerationValue = new Acceleration(aX, aY, aZ);
AngularvelocityValue = new AngularVelocity(gX, gY, gZ);
double x0 = aX * aX + aY * aY + aZ * aZ;
if (x0 < (this.threshold * this.threshold))
{
if (x1 > 0)
{
x1 = 0;
}
return;
}
if (x0 - x1 > (this.threshold * this.threshold))
{
if (originalStep < 0)
{
originalStep = step;
}
currentStep = step - originalStep;
PedometerValue = new PedometerData(step, calorie, distance, duration, currentStep);
step = step + 1;
}
x1 = x0;
}
}
public void HandleDownload()
{
AngularVelocity actual = null;
AngularVelocity expected = new AngularVelocity(
BitConverter.ToSingle(new byte[] { 0x2c, 0x31, 0x60, 0x43 }, 0),
BitConverter.ToSingle(new byte[] { 0xbc, 0xd2, 0x2a, 0x43 }, 0),
BitConverter.ToSingle(new byte[] { 0x1f, 0x03, 0x13, 0x43 }, 0));
loggers[1].Subscribe(data => actual = data.Value <AngularVelocity>());
Acceleration actualAcc = null;
Acceleration expectedAcc = new Acceleration(
BitConverter.ToSingle(new byte[] { 0x00, 0x00, 0x75, 0x3d }, 0),
BitConverter.ToSingle(new byte[] { 0x00, 0x80, 0x9e, 0x3d }, 0),
BitConverter.ToSingle(new byte[] { 0x00, 0xd0, 0x7d, 0x3f }, 0));
loggers[0].Subscribe(data => actualAcc = data.Value <Acceleration>());
platform.sendMockResponse(new sbyte[] { 11, 7, 0x60, -26, 66, 0, 0, -11, 0, 61, 1, 0x62, -26, 66, 0, 0, -35, 15, 0, 0 });
platform.sendMockResponse(new byte[] { 0x0b, 0x07, 0x61, 0x38, 0xc2, 0x01, 0x00, 0xe6, 0x72, 0x8c, 0x57, 0x63, 0x38, 0xc2, 0x01, 0x00, 0x58, 0x4b, 0x00, 0x00 });
Assert.That(actual, Is.EqualTo(expected));
Assert.That(actualAcc, Is.EqualTo(expectedAcc));
}
private bool InitializeParticle(Particle p)
{
Color4 color;
Matrix32 transform;
CalcInitialColorAndTransform(out color, out transform);
float emitterScaleAmount = 1;
Vector2 emitterScale = new Vector2();
emitterScale.X = transform.U.Length;
emitterScale.Y = transform.V.Length;
float crossProduct = Vector2.CrossProduct(transform.U, transform.V);
if (crossProduct < 0.0f)
{
emitterScale.Y = -emitterScale.Y;
}
emitterScaleAmount = Mathf.Sqrt(Math.Abs(crossProduct));
float emitterAngle = transform.U.Atan2Deg;
NumericRange aspectRatioVariationPair = new NumericRange(0, Math.Max(0.0f, AspectRatio.Dispersion));
float zoom = Zoom.NormalRandomNumber(Rng);
float aspectRatio = AspectRatio.Median *
(1 + Math.Abs(aspectRatioVariationPair.NormalRandomNumber(Rng))) /
(1 + Math.Abs(aspectRatioVariationPair.NormalRandomNumber(Rng)));
p.TextureIndex = 0.0f;
p.Velocity = Velocity.NormalRandomNumber(Rng) * emitterScaleAmount;
p.ScaleInitial = emitterScale * ApplyAspectRatio(zoom, aspectRatio);
p.ScaleCurrent = p.ScaleInitial;
p.WindDirection = WindDirection.UniformRandomNumber(Rng);
p.WindAmount = WindAmount.NormalRandomNumber(Rng) * emitterScaleAmount;
p.GravityVelocity = 0.0f;
p.GravityAcceleration = 0.0f;
p.GravityAmount = GravityAmount.NormalRandomNumber(Rng) * emitterScaleAmount;
p.GravityDirection = GravityDirection.NormalRandomNumber(Rng);
p.MagnetAmountInitial = MagnetAmount.NormalRandomNumber(Rng);
p.Lifetime = Math.Max(Lifetime.NormalRandomNumber(Rng), 0.1f);
p.Age = 0.0f;
p.AngularVelocity = AngularVelocity.NormalRandomNumber(Rng);
p.Angle = Orientation.UniformRandomNumber(Rng) + emitterAngle;
p.Spin = Spin.NormalRandomNumber(Rng);
p.ColorInitial = color;
p.ColorCurrent = color;
p.RandomRayDirection = (new NumericRange(0, 360)).UniformRandomNumber(Rng);
p.RandomSplineVertex0 = GenerateRandomMotionControlPoint(ref p.RandomRayDirection);
p.RandomSplineVertex1 = Vector2.Zero;
p.RandomSplineVertex2 = GenerateRandomMotionControlPoint(ref p.RandomRayDirection);
p.RandomSplineVertex3 = GenerateRandomMotionControlPoint(ref p.RandomRayDirection);
p.RandomMotionSpeed = RandomMotionSpeed.NormalRandomNumber(Rng);
p.RandomSplineOffset = 0;
Vector2 position;
switch (Shape)
{
case EmitterShape.Point:
position = 0.5f * Size;
p.RegularDirection = Direction.UniformRandomNumber(Rng) + emitterAngle - 90.0f;
break;
case EmitterShape.Line:
position = new Vector2(Rng.RandomFloat() * Size.X, Size.Y * 0.5f);
p.RegularDirection = Direction.UniformRandomNumber(Rng) + emitterAngle - 90.0f;
break;
case EmitterShape.Ellipse:
float angle = Rng.RandomFloat(0, 360);
Vector2 sincos = Vector2.CosSinRough(angle * Mathf.DegToRad);
position = 0.5f * ((sincos + Vector2.One) * Size);
p.RegularDirection = Direction.UniformRandomNumber(Rng) + emitterAngle - 90 + angle;
break;
case EmitterShape.Area:
position.X = Rng.RandomFloat() * Size.X;
position.Y = Rng.RandomFloat() * Size.Y;
p.RegularDirection = Direction.UniformRandomNumber(Rng) + emitterAngle - 90.0f;
break;
case EmitterShape.Custom:
position = GetPointInCustomShape();
p.RegularDirection = Direction.UniformRandomNumber(Rng) + emitterAngle - 90.0f;
break;
default:
throw new Lime.Exception("Invalid particle emitter shape");
}
p.RegularPosition = transform.TransformVector(position);
if (!TryGetRandomModifier(out p.Modifier))
{
return(false);
}
var animationDuration = AnimationUtils.FramesToSeconds(p.Modifier.Animators.GetOverallDuration());
p.AgeToAnimationTime = (float)(animationDuration / p.Lifetime);
if (EmissionType == EmissionType.Inner)
{
p.RegularDirection += 180;
}
else if ((EmissionType & EmissionType.Inner) != 0)
{
if (Rng.RandomInt(2) == 0)
{
p.RegularDirection += 180;
}
}
else if (EmissionType == 0)
{
return(false);
}
p.FullDirection = p.RegularDirection;
p.FullPosition = p.RegularPosition;
return(true);
}
public void SetZeroVelocities()
{
LinearVelocity.Set(0.0F, 0.0F, 0.0F);
AngularVelocity.Set(0.0F, 0.0F, 0.0F);
}
private void updateMovement(TimeSpan elapsedTime, float acceleration, float leftRight)
{
var t = (float)elapsedTime.NumericValue;
this.speed += new Acceleration(acceleration * 50) * elapsedTime;
this.speed *= Mathf.Pow(1e-3f, t);
this.turnSpeed += AngularAcceleration.FromRadians(leftRight * 5) * elapsedTime;
this.turnSpeed *= Mathf.Pow(1e-7f, t);
this.Direction += this.turnSpeed * (elapsedTime * this.speed.NumericValue);
this.Position += this.Direction * this.speed * elapsedTime;
this.distanceTraveled += this.speed * elapsedTime;
}
/*-------------------------------------
* PRIVATE METHODS
*-----------------------------------*/
private void solveCollision(float dt, Entity entity, Entity entity2, Velocity velocity, Position position, Sprite sprite, Position pos2, Sprite spr2, BoundingRectangle br2)
{
var managers = Engine.getInst().managers;
ParticleManager pm = null;
for (int i = 0; i < managers.Count; i++)
{
if (managers[i].GetType().ToString() == "Breakout.Engine.Managers.ParticleManager")
{
pm = (ParticleManager)managers[i];
}
}
BrickStatus brickStatus = entity2.getComponent <BrickStatus>();
if (brickStatus != null)
{
if (!brickStatus.alive)
{
return;
}
// Kollision med brick
float x = position.x;
float y = position.y;
if (position.x < br2.x - (sprite.texture.Width * 0.5f) || position.x > br2.x + br2.width + (sprite.texture.Width * 0.5f))
{
velocity.x *= -1;
x = (position.x < br2.x - (sprite.texture.Width * 0.5f)) ? x - (sprite.texture.Width * 0.5f) : x + (sprite.texture.Width * 0.5f);
}
else
{
velocity.y *= -1;
y = (position.y < br2.y - (sprite.texture.Height * 0.5f)) ? y - (sprite.texture.Height * 0.5f) : y + (sprite.texture.Height * 0.5f);
}
entity2.getComponent <BrickStatus>().alive = false;
for (int j = 0; j < 100; j++)
{
pm.generateNewParticle(new Vector2(x, y));
}
}
else // Kollision med paddlee
{
if ((position.x < br2.x - (sprite.texture.Width * 0.5f) || position.x > br2.x + br2.width + (sprite.texture.Width * 0.5f)) && position.y > br2.y + (sprite.texture.Height * 0.5f))
{
velocity.x *= -1;
}
else
{
// Pythagoras på X och Y hastigheten för att räkna ut totala hastigheten.
float speedXY = (float)Math.Sqrt((velocity.x * velocity.x) + (velocity.y * velocity.y));
// Räkna ut bollens position relativt center av paddlen, resultat är emellan -1 och +1.
float posX = (position.x - pos2.x) / (spr2.texture.Width * 0.5f);
// Värde mellan 0 och 1 för hur mycket påverkan det ska ha på x-hastighet.
float influenceX = 0.5f;
// Räkna ut nya X-hastigheten baserat på vart den träffar paddlen,
// gör den även relativ till originalhastighet och tweaka efter influenceX ovan.
velocity.x = speedXY * posX * influenceX;
// Baserat på nya x-hastigheten, räkna ut nya y-hastigheten så den totala hastigheten blir samma
// som innan. Återigen pythagoras sats.
velocity.y = (float)Math.Sqrt((speedXY * speedXY) - (velocity.x * velocity.x)) *
(velocity.y > 0 ? -1 : 1);
// Add some bounce on paddle
var velocityPaddle = entity2.getComponent <Velocity>();
velocityPaddle.y += 300.0f;
//Angle angle = entity.getComponent<Angle>();
AngularVelocity av = entity.getComponent <AngularVelocity>();
// Spin ball
av.velocity = 0.03f * posX;
// Hit tilt effect
AngularVelocity avPaddle = entity2.getComponent <AngularVelocity>();
if (position.x < pos2.x - 15.0f || position.x > pos2.x + 15.0f)
{
rotation = position.x < pos2.x ? -1 : 1;
avPaddle.velocity = rotation < 0 ? -0.01f : 0.01f;
}
}
}
// "Clamp"
if (((br2.x - (sprite.texture.Width * 0.5f) + 5) < position.x && position.x < (br2.x + br2.width + (sprite.texture.Width * 0.5f) - 5)) &&
((br2.y - (sprite.texture.Height * 0.5f) + 5) < position.y && position.y < (br2.y + br2.height + (sprite.texture.Height * 0.5f) - 5)))
{
if (Math.Abs(position.x - (br2.x + br2.width + (sprite.texture.Width * 0.5f))) < Math.Abs(position.x - (br2.x - (sprite.texture.Width * 0.5f))))
{
position.x = (br2.x + br2.width + (sprite.texture.Width * 0.5f)) + 1;
}
else
{
position.x = (br2.x - (sprite.texture.Width * 0.5f)) - 1;
}
}
}
internal void ApplyPatch(RigidBodyPatch patch, bool patchVelocities)
{
// Apply any changes made to the state of the mixed reality extension runtime version of the rigid body.
if (patchVelocities)
{
if (patch.Velocity != null && patch.Velocity.IsPatched())
{
_rigidbody.velocity = _rigidbody.velocity.GetPatchApplied(_sceneRoot.TransformDirection(Velocity.ApplyPatch(patch.Velocity).ToVector3()));
}
if (patch.AngularVelocity != null && patch.AngularVelocity.IsPatched())
{
_rigidbody.angularVelocity = _rigidbody.angularVelocity.GetPatchApplied(_sceneRoot.TransformDirection(AngularVelocity.ApplyPatch(patch.AngularVelocity).ToVector3()));
}
}
if (patch.Mass.HasValue)
{
_rigidbody.mass = _rigidbody.mass.GetPatchApplied(Mass.ApplyPatch(patch.Mass));
}
if (patch.DetectCollisions.HasValue)
{
_rigidbody.detectCollisions = _rigidbody.detectCollisions.GetPatchApplied(DetectCollisions.ApplyPatch(patch.DetectCollisions));
}
if (patch.CollisionDetectionMode.HasValue)
{
_rigidbody.collisionDetectionMode = _rigidbody.collisionDetectionMode.GetPatchApplied(CollisionDetectionMode.ApplyPatch(patch.CollisionDetectionMode));
}
if (patch.UseGravity.HasValue)
{
_rigidbody.useGravity = _rigidbody.useGravity.GetPatchApplied(UseGravity.ApplyPatch(patch.UseGravity));
}
if (patch.IsKinematic.HasValue)
{
_rigidbody.isKinematic = _rigidbody.isKinematic.GetPatchApplied(IsKinematic.ApplyPatch(patch.IsKinematic));
}
_rigidbody.constraints = (RigidbodyConstraints)((int)_rigidbody.constraints).GetPatchApplied((int)ConstraintFlags.ApplyPatch(patch.ConstraintFlags));
}
