Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- private Torque GetTorqueVectors()
- {
- ReferenceFrame frame = Part.Vessel.ReferenceFrame;
- double torqueXn = 0;
- double torqueX = 0;
- double torqueZ = 0;
- double torqueZn = 0;
- double torqueY = 0;
- double torqueYn = 0;
- Tuple3 position = Part.Position(frame);
- float thrust = MaxThrust;
- foreach (var thruster in Thrusters)
- {
- double torque = 0;
- double forceX = thruster.ThrustDirection(frame).Item1;
- double forceY = thruster.ThrustDirection(frame).Item2;
- double forceZ = thruster.ThrustDirection(frame).Item3;
- //
- double posX = position.Item1;
- double posY = position.Item2;
- double posZ = position.Item3;
- //Pitch torque, around X (position of RCS module projected on Z):
- torque = posZ * forceY * thrust;
- if (torque > 0) torqueX += torque;
- else torqueXn += torque;
- //Pitch torque, around X (position projected on Y):
- torque = posY * forceZ * thrust;
- if (torque < 0) torqueX += torque;
- else torqueXn += torque;
- //Yaw torque, around Z (position projected on X):
- torque = posX * forceY * thrust;
- if (torque > 0) torqueZn += torque;
- else torqueZ += torque;
- //Yaw torque, around Z (position projected on Y):
- torque = posY * forceX * thrust;
- if (torque < 0) torqueZn += torque;
- else torqueZ += torque;
- //Roll torque, around Y (position projected on Z):
- torque = posZ * forceX * thrust;
- if (torque < 0) torqueY += torque;
- else torqueYn += torque;
- //Roll torque, around Y (position projected on X):
- torque = posX * forceZ * thrust;
- if (torque < 0) torqueYn += torque;
- else torqueY += torque;
- }
- Vector3d posd = new Vector3d(torqueX, torqueZ, torqueY);
- Vector3d negd = new Vector3d(torqueXn, torqueZn, torqueYn);
- return new Torque(posd, negd);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement