public override int GetHashCode()
{
int hash = 1;
if (X != 0D)
{
hash ^= X.GetHashCode();
}
if (Y != 0D)
{
hash ^= Y.GetHashCode();
}
if (Z != 0D)
{
hash ^= Z.GetHashCode();
}
if (Yaw != 0F)
{
hash ^= Yaw.GetHashCode();
}
if (Pitch != 0F)
{
hash ^= Pitch.GetHashCode();
}
return(hash);
}
public override void Read(BinaryReader file, uint size)
{
flags = file.ReadByte();
if ((flags & 1) == 1)
{
X.Read(file, 4);
Y.Read(file, 4);
Z.Read(file, 4);
}
if ((flags & 2) == 2)
{
Pitch.Read(file, 4);
Yaw.Read(file, 4);
Roll.Read(file, 4);
}
if ((flags & 4) == 4)
{
Scale_x.Read(file, 4);
Scale_y.Read(file, 4);
Scale_z.Read(file, 4);
}
}
/// <summary>
/// Gets the hash code
/// </summary>
/// <returns>Hash code</returns>
public override int GetHashCode()
{
unchecked // Overflow is fine, just wrap
{
var hashCode = 41;
// Suitable nullity checks etc, of course :)
if (UserRef != null)
{
hashCode = hashCode * 59 + UserRef.GetHashCode();
}
if (Yaw != null)
{
hashCode = hashCode * 59 + Yaw.GetHashCode();
}
if (Position != null)
{
hashCode = hashCode * 59 + Position.GetHashCode();
}
if (Timestamp != null)
{
hashCode = hashCode * 59 + Timestamp.GetHashCode();
}
return(hashCode);
}
}
/// <summary>
/// Returns true if Location instances are equal
/// </summary>
/// <param name="other">Instance of Location to be compared</param>
/// <returns>Boolean</returns>
public bool Equals(Location other)
{
if (ReferenceEquals(null, other))
{
return(false);
}
if (ReferenceEquals(this, other))
{
return(true);
}
return
((
UserRef == other.UserRef ||
UserRef != null &&
UserRef.Equals(other.UserRef)
) &&
(
Yaw == other.Yaw ||
Yaw != null &&
Yaw.Equals(other.Yaw)
) &&
(
Position == other.Position ||
Position != null &&
Position.Equals(other.Position)
) &&
(
Timestamp == other.Timestamp ||
Timestamp != null &&
Timestamp.Equals(other.Timestamp)
));
}
public void Should_SnapIfLargeTurn()
{
Assert.AreEqual(
Math.PI,
Yaw.RotateBy(0, Math.PI, 0.1, 1),
TOLERANCE
);
}
public void Should_IgnoreYawRateSign()
{
Assert.AreEqual(
0.1,
Yaw.RotateBy(0, Math.PI / 2, -0.1, 1),
TOLERANCE
);
}
public XElement SerializeXML(string Name)
{
return(new XElement(Name,
new XElement("pitch", Pitch.ToString(NumberFormatInfo.InvariantInfo)),
new XElement("yaw", Yaw.ToString(NumberFormatInfo.InvariantInfo)),
new XElement("roll", Roll.ToString(NumberFormatInfo.InvariantInfo)),
new XAttribute("class", "Rotator")));
}
public override int GetHashCode()
{
unchecked {
var hashCode = Roll.GetHashCode();
hashCode = (hashCode * 397) ^ Yaw.GetHashCode();
hashCode = (hashCode * 397) ^ Pitch.GetHashCode();
return(hashCode);
}
}
public override int GetHashCode()
{
var hashCode = -1766562337;
hashCode = hashCode * -1521134295 + Yaw.GetHashCode();
hashCode = hashCode * -1521134295 + Pitch.GetHashCode();
hashCode = hashCode * -1521134295 + Roll.GetHashCode();
hashCode = hashCode * -1521134295 + TimeMs.GetHashCode();
return(hashCode);
}
public override Pose Output()
{
Yaw.Calculate();
Pitch.Calculate();
Roll.Calculate();
Distance.Calculate();
Target.Calculate();
float yaw = 0f;
float pitch = 0f;
float roll = 0f;
float dist = 0f;
Vector3 tar = Vector3.zero;
Pose pose = Pose.Default;
if (modifier != null)
{
pose = modifier.Prev.value;
yaw = pose.yaw;
pitch = pose.pitch;
roll = pose.roll;
dist = 0f;
tar = pose.position;
}
if (!Yaw.value.isNull)
{
yaw = Yaw.value.value_f;
}
if (!Pitch.value.isNull)
{
pitch = Pitch.value.value_f;
}
if (!Roll.value.isNull)
{
roll = Roll.value.value_f;
}
if (!Distance.value.isNull)
{
dist = Distance.value.value_f;
}
if (!Target.value.isNull)
{
tar = Target.value.value_v;
}
pose.eulerAngles = new Vector3(-pitch, yaw, roll);
Vector3 forward = pose.rotation * Vector3.forward;
pose.position = tar - dist * forward;
return(pose);
}
//public Vector3 GetDirection()
//{
// double pitch = Pitch.ToRadians();
// double yaw = Yaw.ToRadians();
// double y = -Math.Sin(pitch);
// double xz = Math.Cos(pitch);
// double x = -xz * Math.Sin(yaw);
// double z = xz * Math.Cos(yaw);
// return new Vector3((float)x, (float)y, (float)z);
//}
public Vector3 GetDirection()
{
Vector3 vector = new Vector3();
double pitch = Pitch.ToRadians();
double yaw = Yaw.ToRadians();
vector.X = (float)(-Math.Sin(yaw) * Math.Cos(pitch));
vector.Y = (float)-Math.Sin(pitch);
vector.Z = (float)(Math.Cos(yaw) * Math.Cos(pitch));
return(vector);
}
public override int GetHashCode()
{
unchecked
{
int hashCode = X.GetHashCode();
hashCode = (hashCode * 397) ^ Y.GetHashCode();
hashCode = (hashCode * 397) ^ Z.GetHashCode();
hashCode = (hashCode * 397) ^ Yaw.GetHashCode();
hashCode = (hashCode * 397) ^ Pitch.GetHashCode();
hashCode = (hashCode * 397) ^ Roll.GetHashCode();
return(hashCode);
}
}
public void Should_Interpolate()
{
Assert.AreEqual(
0.1,
Yaw.RotateBy(0, Math.PI / 2.0, 0.1, 1),
TOLERANCE
);
Assert.AreEqual(
-0.05,
Yaw.RotateBy(0, -0.1, 0.05, 1),
TOLERANCE
);
}
public void Should_Update_ByCorrectAmount()
{
Assert.AreEqual(
Math.PI / 2,
Yaw.RotateBy(0, Math.PI / 2, Math.PI / 2, 1.0),
TOLERANCE
);
Assert.AreEqual(
0.0,
Yaw.RotateBy(Math.PI / 2, 0, Math.PI / 2, 1.0),
TOLERANCE
);
}
public XElement ToXml()
{
var elm = new XElement("joint");
elm.Add(Yaw.ToXml("yaw"), Pitch.ToXml("pitch"), Roll.ToXml("roll"));
elm.Add(ParentOffset.ToXml("offset"));
elm.Add(new XElement("length", Length));
elm.Add(Size.ToXml("size"));
elm.Add(Color.ToVector3().ToXml("color"));
elm.Add(new XElement("visible", Visible));
foreach (var item in Children)
{
elm.Add(item.ToXml());
}
return(elm);
}
public override int GetHashCode()
{
int hash = 1;
if (EntityId != 0UL)
{
hash ^= EntityId.GetHashCode();
}
if (FlagPosition != false)
{
hash ^= FlagPosition.GetHashCode();
}
if (X != 0D)
{
hash ^= X.GetHashCode();
}
if (Y != 0D)
{
hash ^= Y.GetHashCode();
}
if (Z != 0D)
{
hash ^= Z.GetHashCode();
}
if (FlagRotation != false)
{
hash ^= FlagRotation.GetHashCode();
}
if (Yaw != 0F)
{
hash ^= Yaw.GetHashCode();
}
if (Pitch != 0F)
{
hash ^= Pitch.GetHashCode();
}
if (FlagMeta != false)
{
hash ^= FlagMeta.GetHashCode();
}
if (meta_ != null)
{
hash ^= Meta.GetHashCode();
}
return(hash);
}
public Tuple <short[], short[]> ApplyCalToInReport(byte[] inputReport, int startOffs = 0)
{
int idx = startOffs + 13; //first byte of gyro is here
var prevVals = new short[6];
var currVals = new short[6];
for (int i = 0; i < 6; i++)
{
short axisVal = (short)(((ushort)inputReport[idx + 1] << 8) | (ushort)inputReport[idx + 0]);
prevVals[i] = axisVal;
if (i == 0)
{
axisVal = Pitch.apply(axisVal);
}
else if (i == 1)
{
axisVal = Yaw.apply(axisVal);
}
else if (i == 2)
{
axisVal = Roll.apply(axisVal);
}
else if (i == 3)
{
axisVal = X.apply(axisVal);
}
else if (i == 4)
{
axisVal = Y.apply(axisVal);
}
else if (i == 5)
{
axisVal = Z.apply(axisVal);
}
currVals[i] = axisVal;
//put it back into the input report (little endian short)
inputReport[idx++] = (byte)(((ushort)axisVal >> 0) & 0xFF);
inputReport[idx++] = (byte)(((ushort)axisVal >> 8) & 0xFF);
}
return(new Tuple <short[], short[]>(prevVals, currVals));
}
public override int GetHashCode()
{
int hash = 1;
if (header_ != null)
{
hash ^= Header.GetHashCode();
}
if (MeasureType != 0)
{
hash ^= MeasureType.GetHashCode();
}
if (FrameType != 0)
{
hash ^= FrameType.GetHashCode();
}
if (position_ != null)
{
hash ^= Position.GetHashCode();
}
if (velocity_ != null)
{
hash ^= Velocity.GetHashCode();
}
if (Yaw != 0D)
{
hash ^= Yaw.GetHashCode();
}
if (ZoneId != 0)
{
hash ^= ZoneId.GetHashCode();
}
if (IsHaveVariance != false)
{
hash ^= IsHaveVariance.GetHashCode();
}
if (IsGnssDoubleAntenna != false)
{
hash ^= IsGnssDoubleAntenna.GetHashCode();
}
hash ^= measureCovar_.GetHashCode();
return(hash);
}
public override void Write(BinaryWriter file)
{
flags = 0;
if (X.val != 0 || Y.val != 0 || Z.val != 0)
{
flags |= 1;
}
if (Pitch.val != 0 || Yaw.val != 0 || Roll.val != 0 || W.val != 1)
{
flags |= 2;
}
if (Scale_x.val != 0 || Scale_y.val != 0 || Scale_z.val != 0)
{
flags |= 4;
}
file.Write(flags);
if ((flags & 1) == 1)
{
X.Write(file);
Y.Write(file);
Z.Write(file);
}
if ((flags & 2) == 2)
{
Pitch.Write(file);
Yaw.Write(file);
Roll.Write(file);
W.Write(file);
}
if ((flags & 4) == 4)
{
Scale_x.Write(file);
Scale_y.Write(file);
Scale_z.Write(file);
}
}
public override int GetHashCode()
{
int hash = 1;
if (EntityId != 0UL)
{
hash ^= EntityId.GetHashCode();
}
if (EntityType != 0)
{
hash ^= EntityType.GetHashCode();
}
if (X != 0D)
{
hash ^= X.GetHashCode();
}
if (Y != 0D)
{
hash ^= Y.GetHashCode();
}
if (Z != 0D)
{
hash ^= Z.GetHashCode();
}
if (Yaw != 0F)
{
hash ^= Yaw.GetHashCode();
}
if (Pitch != 0F)
{
hash ^= Pitch.GetHashCode();
}
if (meta_ != null)
{
hash ^= Meta.GetHashCode();
}
return(hash);
}
public void ApplyCalToInReport(byte[] inputReport, int startOffs = 0)
{
int idx = startOffs + 13; //first byte of gyro is here
for (int i = 0; i < 6; i++)
{
short axisVal = (short)(((ushort)inputReport[idx + 1] << 8) | (ushort)inputReport[idx + 0]);
if (i == 0)
{
axisVal = Pitch.apply(axisVal);
}
else if (i == 1)
{
axisVal = Yaw.apply(axisVal);
}
else if (i == 2)
{
axisVal = Roll.apply(axisVal);
}
else if (i == 3)
{
axisVal = X.apply(axisVal);
}
else if (i == 4)
{
axisVal = Y.apply(axisVal);
}
else if (i == 5)
{
axisVal = Z.apply(axisVal);
}
//put it back into the input report (little endian short)
inputReport[idx++] = (byte)(((ushort)axisVal >> 0) & 0xFF);
inputReport[idx++] = (byte)(((ushort)axisVal >> 8) & 0xFF);
}
}
/// <summary>
/// Applies left/right/forward movement based on the current entity's yaw
/// </summary>
/// <param name="strafMovement"></param>
/// <param name="forwardMovement"></param>
/// <param name="friction"></param>
protected void ApplyStrafingToVelocity(double strafMovement, double forwardMovement, double friction)
{
double distance = Math.Sqrt(strafMovement * strafMovement + forwardMovement * forwardMovement);
if (distance < 0.01)
{
return;
}
if (distance < 1.0)
{
distance = 1.0;
}
distance = friction / distance;
strafMovement *= distance;
forwardMovement *= distance;
// Based on current yaw and xMovement and zMovement, determine the velocity along X / Z axis
double yawSin = Math.Sin(Yaw.ToRadians());
double yawCos = Math.Cos(Yaw.ToRadians());
this.Velocity.X += strafMovement * yawCos - forwardMovement * yawSin;
this.Velocity.Z += forwardMovement * yawCos + strafMovement * yawSin;
}
public override int GetHashCode()
{
return(Roll.GetHashCode() ^ Pitch.GetHashCode() ^ Yaw.GetHashCode());
}
public override int GetHashCode() => 13 * 17 + Yaw.GetHashCode() * 17 + Pitch.GetHashCode() * 17 + Roll.GetHashCode();
public bool Equals(Euler3 other) => Yaw.Equals(other.Yaw) && Pitch.Equals(other.Pitch) && Roll.Equals(other.Roll);
public override String ToString()
{
String value = "Roll: " + Roll.ToString("0.000") + ", Pitch: " + Pitch.ToString("0.000") + ", Yaw: " + Yaw.ToString("0.000") + ", Gaz: " + Gaz.ToString("0.000");
if (CameraSwap)
{
value += ", Change Camera";
}
if (TakeOff)
{
value += ", Take Off";
}
if (Land)
{
value += ", Land";
}
if (Hover)
{
value += ", Hover";
}
if (Emergency)
{
value += ", Emergency";
}
if (FlatTrim)
{
value += ", Flat Trim";
}
return(value);
}
protected bool Equals(HeadPoseData other)
{
return(X.Equals(other.X) && Y.Equals(other.Y) && Z.Equals(other.Z) && Yaw.Equals(other.Yaw) && Pitch.Equals(other.Pitch) && Roll.Equals(other.Roll));
}
/// <summary>
/// Returns a string that represents the current object.
/// </summary>
/// <returns>A string that represents the current object.</returns>
/// <since_tizen> 5 </since_tizen>
public override string ToString() =>
$"Yaw={ Yaw.ToString() }, Pitch={ Pitch.ToString() }";
public bool Equals(RealEulerAngles3d other) => Yaw.Equals(other.Yaw) && Pitch.Equals(other.Pitch) && Roll.Equals(other.Roll);
protected bool Equals(Rotation other)
{
return(Roll.Equals(other.Roll) && Yaw.Equals(other.Yaw) && Pitch.Equals(other.Pitch));
}
