public void UpdateCurve(PVector3 velocity, int cElementCount = PSettings.CurveElementCount)
{
var ElInfo = GetEllipseInfo(Body.LocalPosition, velocity, Body.Mass, Body.GravityBody.Mass);
Curve = MakeCurve(ElInfo.a, ElInfo.b, Body.LocalPosition, velocity, Body.GravityBody.Position, Body.GravityBody.Mass, cElementCount);
//Debug.Log("e: "+ Curve.e + " p: "+Curve.p + " iA: "+Curve.PositionAngle);
}
public static PQuaternion CreateFromAxisAngle(PVector3 axis, double angle)
{
axis.Normalize();
double sin_a = Math.Sin(angle / 2.0);
return(new PQuaternion(axis.X * sin_a, axis.Y * sin_a, axis.Z * sin_a, Math.Cos(angle / 2.0)));
}
public PQuaternion(PVector3 vectorPart, double scalarPart)
{
this.X = vectorPart.X;
this.Y = vectorPart.Y;
this.Z = vectorPart.Z;
this.W = scalarPart;
}
internal static float Length(PVector3 vec)
{
float lenSQ = (vec.x * vec.x) +
(vec.y * vec.y) + (vec.z * vec.z);
return((float)Math.Sqrt(lenSQ));
}
public PRigidbody(PVector3 position, PWorld world)
{
Position = position;
Rotation = PQuaternion.identity;
this.world = world;
Trajectory = new PTrajectory(this, world);
}
internal static float Distance(PVector3 vecA, PVector3 vecB)
{
vecA.x -= vecB.x;
vecA.y -= vecB.y;
vecA.z -= vecB.z;
return(Length(vecA));
}
internal static PVector3 AdjustUp(PVector3 pos)
{
PVector3 ret = pos;
ret.y += 0.1f;
return(ret);
}
public PVector3 GetVelocityMagnitude(PVector3 Position, double angle)
{
double gm = PWorld.G * Body.GravityBody.Mass;
double k = Math.Sqrt(gm / Curve.p);
PVector3 Vn = PVector3.Cross(Curve.Lv, Position).normalized *(k * (1 + Curve.e * Math.Cos(angle)));
PVector3 Vr = Position.normalized * (k * Curve.e * Math.Sin(angle));
return(Vn + Vr);
}
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 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 PVector3 GetMotionValue(string motionId, PVector3 linearValue, int maxSample = MotionItem.DefaultMaxSample, float tolerance = MotionItem.DefaultMaxTolerance)
{
MotionItem motion = GetMotion(motionId);
return(new PVector3(
motion.GetMotionValue(linearValue.x, maxSample, tolerance),
motion.GetMotionValue(linearValue.y, maxSample, tolerance),
motion.GetMotionValue(linearValue.z, maxSample, tolerance)
));
}
internal static PVector3 Add(PVector3 vecA, PVector3 vecB)
{
PVector3 ret;
ret.x = vecA.x + vecB.x;
ret.y = vecA.y + vecB.y;
ret.z = vecA.z + vecB.z;
return(ret);
}
public PRigidbody AddRigidBody(PVector3 position, PVector3 velocity, double mass)
{
var body = new PRigidbody(position, this)
{
Mass = mass
};
RigidBodies.Add(body);
CheckGRBDistances();
body.Trajectory.UpdateCurve(velocity);
return(body);
}
void UpdateForces(double step)
{
PVector3 force = PVector3.zero;
foreach (var f in forces)
{
force += f;
}
if (force != PVector3.zero)
{
newVelocity += force * step + Velocity;
}
}
public Pattern3D(Pattern _pattern)
{
pattern = _pattern;
state = State.Initialized;
scale = new PVector3();
offset = new PVector3();
rot = new PVector3();
pivot = new PVector3();
deltaSpeed = new PMatrix4x4(Matrix4x4.zero);
deltaOffset = new PMatrix4x4(Matrix4x4.zero);
deltaAmp = new PVector3(Vector3.zero);
deltaPivot = new PVector3(Vector3.zero);
tagStr = string.Empty;
}
//Рассылка координат
void SendUpdates()
{
foreach (var ag in agents)
{
if (!ag.Key.SceneReady)
{
continue;
}
foreach (var rb in GameObjects.Keys)
{
if (rb.RBody.Trajectory.Curve == null)
{
continue;
}
ag.Key.SendClear(
CommandBuilder.Build(new string[]
{
"042", rb.ID.ToString(), rb.RBody.Position.X.ToString(), rb.RBody.Position.Y.ToString(), rb.RBody.Position.Z.ToString(), rb.RBody.Rotation.X.ToString(), rb.RBody.Rotation.Y.ToString(), rb.RBody.Rotation.Z.ToString(), rb.RBody.Rotation.W.ToString()
}));
ag.Key.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()
}));
rb.RBody.TUpdated = false;
PVector3 up = rb.RBody.Rotation * PVector3.up;
ag.Key.SendClear(
CommandBuilder.Build(new string[]
{
"043", rb.ID.ToString(), up.X.ToString(), up.Y.ToString(), up.Z.ToString()
}));
if (rb.Owner == ag.Key && (rb.RBody.GravityBody.Position - rb.RBody.Position).magnitude < 0.010)
{
ag.Key.Send("051");
}
}
}
}
void CheckGRBD()
{
Pair <double, PGravityBody> force = new Pair <double, PGravityBody>(0, null);
foreach (var gb in world.Gravitybodies)
{
double f = PWorld.G * gb.Mass * Mass / (Position - gb.Position).sqrMagnitude;
force = f > force.a ? new Pair <double, PGravityBody>(f, gb) : force;
}
if (GravityBody == force.b)
{
return;
}
var vel = Velocity;
GravityBody = force.b;
newVelocity = vel;
}
public virtual void Update(double step)
{
if (Trajectory?.Curve == null)
{
return;
}
if (forces.Count > 0)
{
UpdateForces(step);
}
if (!(this is PGravityBody))
{
CheckGRBD();
}
if (newVelocity != PVector3.zero)
{
Trajectory.UpdateCurve(newVelocity);
newVelocity = PVector3.zero;
TUpdated = true;
}
Random r = new Random();
double alpha = GetAlphaDt(Trajectory.Curve.PositionTime + step);
while (double.IsNaN(alpha))
{
alpha = GetAlphaDt(Trajectory.Curve.PositionTime + step + r.NextDouble() / 100);
}
var pos = Position;
Position = Trajectory.Curve.RFuncAlphaV3(alpha) + GravityBody.Position;
var dpos = Position - pos;
if (this is PGravityBody)
{
var rbs = world.RigidBodies.FindAll(x => x.GravityBody == (PGravityBody)this);
foreach (var rb in rbs)
{
rb.Position += dpos;
}
}
Trajectory.Curve.PositionAngle = alpha;
Trajectory.Curve.PositionTime += step;
}
public PGravityBody AddGravityBody(PVector3 position, PVector3 speed, string name, double mass)
{
var body = new PGravityBody(name, position, this)
{
Mass = mass
};
if (Gravitybodies.Count > 0)
{
body.GravityBody = Gravitybodies[0];
}
if (speed != PVector3.zero)
{
body.Trajectory.UpdateCurve(speed);
}
Gravitybodies.Add(body);
Gravitybodies.Sort((x, y) => (int)(x.CatchDistance - y.CatchDistance));
return(body);
}
internal bool bCheckNear(PVector3 playerPos, bool bTyada)
{
if (bTyada)
{
float dist = VecStuff.Distance(playerPos, mTyadaPos);
if (dist < mNearDistance)
{
return(true);
}
}
else
{
float dist = VecStuff.Distance(playerPos, mCastaePos);
if (dist < mNearDistance)
{
return(true);
}
}
return(false);
}
public static Pair <double, double> GetEllipseInfo(PVector3 position, PVector3 speed, double mass, double Mass)
{
double E = mass * speed.magnitude * speed.magnitude / 2 - PWorld.G * Mass * mass / position.magnitude;
double L = PVector3.Cross(position, (mass * speed)).magnitude;
double e = Math.Sqrt(1 + 2 * E * L * L / (PWorld.G * PWorld.G * Mass * Mass * mass * mass * mass));
double p = L * L / (PWorld.G * Mass * mass * mass);
//double mu = PWorld.G*Mass;
//double p = PVector3.Cross(position, speed).sqrMagnitude/mu;
//double h = speed.sqrMagnitude - (2*mu)/position.magnitude;
//double e = Math.Sqrt(1 + h*p/mu);
return(new Pair <double, double>(e, p));
}
//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()
}));
}
public static double Dot(PVector3 v1, PVector3 v2)
{
return(v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z);
}
public static double Angle(PVector3 v1, PVector3 v2)//RADIANS !!!!
{
return(Math.Acos(Dot(v1, v2) / (v1.Length() * v2.Length())));
}
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 PVector3 Cross(PVector3 v1, PVector3 v2)
{
return(-new PVector3(v1.Y * v2.Z - v1.Z * v2.Y, v1.Z * v2.X - v1.X * v2.Z, v1.X * v2.Y - v1.Y * v2.X));
}
public static Vector3 GetVector3(PVector3 aVector)
{
return(new Vector3(aVector.x, aVector.y, aVector.z));
}
bool IsZero(PVector3 aVector)
{
return(aVector.x == 0 && aVector.y == 0 && aVector.z == 0);
}
bool IsNearPos(GlobalStructureList aGlobalStructureList, TeleporterData aTarget, PVector3 aTestPos)
{
var StructureInfo = SearchEntity(aGlobalStructureList, aTarget.Id);
if (StructureInfo == null)
{
log($"TargetStructure missing:{aTarget.Id} pos={aTarget.Position.String()}", LogLevel.Error);
return(false);
}
var StructureRotation = GetMatrix4x4(GetVector3(StructureInfo.Data.rot));
var TeleporterPos = Vector3.Transform(aTarget.Position, StructureRotation) + GetVector3(StructureInfo.Data.pos);
var Distance = Math.Abs(Vector3.Distance(TeleporterPos, GetVector3(aTestPos)));
log($"FoundTarget:{StructureInfo.Data.id}/{StructureInfo.Data.type} pos={StructureInfo.Data.pos.String()} TeleportPos={TeleporterPos.String()} TEST {aTestPos.String()} => {Distance}", LogLevel.Message);
return(Distance < 4);
}
