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