public static void Slerp(ref PQuaternion PQuaternion1, ref PQuaternion PQuaternion2, double amount, out PQuaternion result)
{
double q2, q3;
double q4 = (PQuaternion1.X * PQuaternion2.X) + (PQuaternion1.Y * PQuaternion2.Y) + (PQuaternion1.Z * PQuaternion2.Z) + (PQuaternion1.W * PQuaternion2.W);
bool flag = false;
if (q4 < 0.0F)
{
flag = true;
q4 = -q4;
}
if (q4 > 0.999999F)
{
q3 = 1.0F - amount;
q2 = flag ? -amount : amount;
}
else
{
double q5 = (double)System.Math.Acos((double)q4);
double q6 = (double)(1.0 / System.Math.Sin((double)q5));
q3 = (double)System.Math.Sin((double)((1.0F - amount) * q5)) * q6;
q2 = flag ? (double)-System.Math.Sin((double)(amount * q5)) * q6 : (double)System.Math.Sin((double)(amount * q5)) * q6;
}
result.X = (q3 * PQuaternion1.X) + (q2 * PQuaternion2.X);
result.Y = (q3 * PQuaternion1.Y) + (q2 * PQuaternion2.Y);
result.Z = (q3 * PQuaternion1.Z) + (q2 * PQuaternion2.Z);
result.W = (q3 * PQuaternion1.W) + (q2 * PQuaternion2.W);
}
public static void Conjugate(ref PQuaternion value, out PQuaternion result)
{
result.X = -value.X;
result.Y = -value.Y;
result.Z = -value.Z;
result.W = value.W;
}
public static void Negate(ref PQuaternion PQuaternion, out PQuaternion result)
{
result.X = -PQuaternion.X;
result.Y = -PQuaternion.Y;
result.Z = -PQuaternion.Z;
result.W = -PQuaternion.W;
}
public static void CreateFromYawPitchRoll(double yaw, double pitch, double roll, out PQuaternion result)
{
result.X = (((double)Math.Cos((double)(yaw * 0.5f)) * (double)Math.Sin((double)(pitch * 0.5f))) * (double)Math.Cos((double)(roll * 0.5f))) + (((double)Math.Sin((double)(yaw * 0.5f)) * (double)Math.Cos((double)(pitch * 0.5f))) * (double)Math.Sin((double)(roll * 0.5f)));
result.Y = (((double)Math.Sin((double)(yaw * 0.5f)) * (double)Math.Cos((double)(pitch * 0.5f))) * (double)Math.Cos((double)(roll * 0.5f))) - (((double)Math.Cos((double)(yaw * 0.5f)) * (double)Math.Sin((double)(pitch * 0.5f))) * (double)Math.Sin((double)(roll * 0.5f)));
result.Z = (((double)Math.Cos((double)(yaw * 0.5f)) * (double)Math.Cos((double)(pitch * 0.5f))) * (double)Math.Sin((double)(roll * 0.5f))) - (((double)Math.Sin((double)(yaw * 0.5f)) * (double)Math.Sin((double)(pitch * 0.5f))) * (double)Math.Cos((double)(roll * 0.5f)));
result.W = (((double)Math.Cos((double)(yaw * 0.5f)) * (double)Math.Cos((double)(pitch * 0.5f))) * (double)Math.Cos((double)(roll * 0.5f))) + (((double)Math.Sin((double)(yaw * 0.5f)) * (double)Math.Sin((double)(pitch * 0.5f))) * (double)Math.Sin((double)(roll * 0.5f)));
}
public PRigidbody(PVector3 position, PWorld world)
{
Position = position;
Rotation = PQuaternion.identity;
this.world = world;
Trajectory = new PTrajectory(this, world);
}
public bool Equals(PQuaternion other)
{
if ((X == other.X) && (Y == other.Y) && (Z == other.Z))
{
return(W == other.W);
}
return(false);
}
public static PQuaternion Add(PQuaternion PQuaternion1, PQuaternion PQuaternion2)
{
PQuaternion1.X += PQuaternion2.X;
PQuaternion1.Y += PQuaternion2.Y;
PQuaternion1.Z += PQuaternion2.Z;
PQuaternion1.W += PQuaternion2.W;
return(PQuaternion1);
}
//public static PVector3 operator *(PVector3 v, PQuaternion q)
//{
// PVector3 t = 2 * PVector3.Cross(q.XYZ, v);
// return v + q.W * t + PVector3.Cross(q.XYZ, t);
//}
public static PVector3 operator *(PQuaternion q, PVector3 v)
{
PVector3 res;
var t = PQuaternion.QMulV(q, v);
t = t * PQuaternion.Inverse(q);
res = t.XYZ;
return(res);
}
public static void CreateFromAxisAngle(ref PVector3 axis, double angle, out PQuaternion result)
{
double sin_a = (double)Math.Sin(angle / 2.0f);
result.X = axis.X * sin_a;
result.Y = axis.Y * sin_a;
result.Z = axis.Z * sin_a;
result.W = (double)Math.Cos(angle / 2.0f);
}
public static void Normalize(ref PQuaternion PQuaternion, out PQuaternion result)
{
double f1 = 1.0F / (double)System.Math.Sqrt((double)((PQuaternion.X * PQuaternion.X) + (PQuaternion.Y * PQuaternion.Y) + (PQuaternion.Z * PQuaternion.Z) + (PQuaternion.W * PQuaternion.W)));
result.X = PQuaternion.X * f1;
result.Y = PQuaternion.Y * f1;
result.Z = PQuaternion.Z * f1;
result.W = PQuaternion.W * f1;
}
public static PQuaternion Conjugate(PQuaternion value)
{
PQuaternion PQuaternion;
PQuaternion.X = -value.X;
PQuaternion.Y = -value.Y;
PQuaternion.Z = -value.Z;
PQuaternion.W = value.W;
return(PQuaternion);
}
public static PQuaternion QMulV(PQuaternion a, PVector3 b)
{
PQuaternion res;
res.W = -a.X * b.X - a.Y * b.Y - a.Z * b.Z;
res.X = a.W * b.X + a.Y * b.Z - a.Z * b.Y;
res.Y = a.W * b.Y - a.X * b.Z + a.Z * b.X;
res.Z = a.W * b.Z + a.X * b.Y - a.Y * b.X;
return(res);
}
public static PQuaternion Negate(PQuaternion PQuaternion)
{
PQuaternion result;
result.X = -PQuaternion.X;
result.Y = -PQuaternion.Y;
result.Z = -PQuaternion.Z;
result.W = -PQuaternion.W;
return(result);
}
public void PlayerRotate(Agent agent, string[] info)
{
Console.WriteLine("045 run");
PQuaternion rot = new PQuaternion(double.Parse(info[1]), double.Parse(info[2]), double.Parse(info[3]),
double.Parse(info[4]));
var go = GameObjects.Keys.ToList().Find(x => x.Owner == agent);
go.RBody.Rotation = rot;
Console.WriteLine("045 end");
}
public static PQuaternion Inverse(PQuaternion PQuaternion)
{
PQuaternion result;
double m1 = 1.0F / ((PQuaternion.X * PQuaternion.X) + (PQuaternion.Y * PQuaternion.Y) + (PQuaternion.Z * PQuaternion.Z) + (PQuaternion.W * PQuaternion.W));
result.X = -PQuaternion.X * m1;
result.Y = -PQuaternion.Y * m1;
result.Z = -PQuaternion.Z * m1;
result.W = PQuaternion.W * m1;
return(result);
}
public static PQuaternion Normalize(PQuaternion PQuaternion)
{
PQuaternion result;
double f1 = 1.0F / (double)System.Math.Sqrt((double)((PQuaternion.X * PQuaternion.X) + (PQuaternion.Y * PQuaternion.Y) + (PQuaternion.Z * PQuaternion.Z) + (PQuaternion.W * PQuaternion.W)));
result.X = PQuaternion.X * f1;
result.Y = PQuaternion.Y * f1;
result.Z = PQuaternion.Z * f1;
result.W = PQuaternion.W * f1;
return(result);
}
//041 - игрок просто космос
public void PlayerSpace(Agent agent, string[] info)
{
var rb = agent.gameobject;
PVector3 pos = new PVector3(double.Parse(info[1]), double.Parse(info[2]), double.Parse(info[3]));
PQuaternion rot = new PQuaternion(double.Parse(info[4]), double.Parse(info[5]), double.Parse(info[6]),
double.Parse(info[7]));
PVector3 speed = new PVector3(double.Parse(info[8]), double.Parse(info[9]), double.Parse(info[10]));
world.DeleteRigidBody(rb.RBody);
rb.RBody = world.AddRigidBody(pos, speed, 100);
rb.RBody.Rotation = rot;
rb.RBody.forces.Add(new PVector3(0, 0, 0)
{
Tag = "March"
});
agent.SceneReady = true;
foreach (var go in GameObjects.Keys)
{
if (go == rb)
{
continue;
}
agent.Send(CommandBuilder.Build(new string[]
{
"040", go.ID.ToString(), go.Name, go.RBody.Position.X.ToString(), go.RBody.Position.Y.ToString(), go.RBody.Position.Z.ToString(), go.RBody.Rotation.X.ToString(), go.RBody.Rotation.Y.ToString(), go.RBody.Rotation.Z.ToString(), go.RBody.Rotation.W.ToString()
}));
}
Thread.Sleep(500);
agent.SendClear(
CommandBuilder.Build(new string[]
{
"050", rb.ID.ToString(), rb.RBody.Trajectory.Curve.e.ToString(),
rb.RBody.Trajectory.Curve.p.ToString(), rb.RBody.Velocity.X.ToString(),
rb.RBody.Velocity.Y.ToString(), rb.RBody.Velocity.Z.ToString(),
GameObjects.Keys.ToList().Find(x => x.RBody == rb.RBody.GravityBody).ID.ToString()
}));
}
PVector3 rot(PVector3 v, PQuaternion q)
{
PVector3 t = 2 * PVector3.Cross(q.XYZ, v);
return(v + q.W * t + PVector3.Cross(q.XYZ, t));
}
public PVector3 RotateAround(PVector3 axis, double angle) //RADIANS
{
return(rot(this, PQuaternion.CreateFromAxisAngle(axis, angle)));
}
public static double Dot(PQuaternion PQuaternion1, PQuaternion PQuaternion2)
{
return((PQuaternion1.X * PQuaternion2.X) + (PQuaternion1.Y * PQuaternion2.Y) + (PQuaternion1.Z * PQuaternion2.Z) + (PQuaternion1.W * PQuaternion2.W));
}
public static void Dot(ref PQuaternion PQuaternion1, ref PQuaternion PQuaternion2, out double result)
{
result = (PQuaternion1.X * PQuaternion2.X) + (PQuaternion1.Y * PQuaternion2.Y) + (PQuaternion1.Z * PQuaternion2.Z) + (PQuaternion1.W * PQuaternion2.W);
}
