BarMagnet Mod [Besiege]

using System;
using System.Collections.Generic;
using spaar.ModLoader;
using TheGuysYouDespise;
using UnityEngine;

namespace Blocks {
    public class BarMagnetLoader : spaar.ModLoader.Mod {
        public override string Name { get { return "BarMagnet"; } }
        public override string DisplayName { get { return "BarMagnet"; } }
        public override string Author { get { return "ITR"; } }
        public override Version Version { get { return new Version(1,0); } }


        public override void OnLoad() {
            Game.OnSimulationToggle += ResetList;
            spaar.Commands.RegisterHelpMessage("BarMagnet commands:\n   MaxDistance [int]\n MagnetForce [float]");
            spaar.Commands.RegisterCommand("MaxDistance",(args,namedArgs) => {
                try {
                    BarMagnet.maxDistance = float.Parse(args[0]);
                }
                catch {
                    return "MaxDistance is "+BarMagnet.maxDistance.ToString();
                }
                return "MaxDistance was set to "+BarMagnet.maxDistance.ToString();
            },"Changes the MaxDistance of the magnets (lower numbers make it less laggy)");

            spaar.Commands.RegisterCommand("MagnetForce",(args,namedArgs) => {
                try {
                    BarMagnet.forceMultiplier = float.Parse(args[0]);
                }
                catch {
                    return "MagnetForce is "+BarMagnet.forceMultiplier.ToString();
                }
                return "MagnetForce was set to "+BarMagnet.forceMultiplier.ToString();
            },"Changes the force of the magnets");


            /// <Block-loading-info>
            /// Place .obj file in Mods/Blocks/Obj
            /// Place texture in Mods/Blocks/Textures
            /// Place any additional resources in Mods/Blocks/Resources
            /// </Block-loading-info>

            #region set offset position, scale and rotation for the mesh
            VisualOffset offset = new VisualOffset(new Vector3(1f,1f,1f),Vector3.zero,Vector3.zero);
            #endregion

            #region set transform properties of the icon
            Icon icon = new Icon(1f,new Vector3(0f,0f,0f),new Vector3(0f,0f,0f));
            #endregion
            #region add a compound collider
            List<ColliderComposite> compoundCollider = new List<ColliderComposite> { new ColliderComposite(new Vector3(0.9f,0.9f,1.8f),new Vector3(0f,0f,1.05f),new Vector3(0f,0f,0f)) };
            #endregion
            #region add a set of block properties
            BlockProperties blockProp = new BlockProperties().ToggleModeEnabled("Inverted",false);
            #endregion
            #region add any needed resources for the block
            List<NeededResource> neededResources = new List<NeededResource>();
            #endregion
            #region add scripts to the block
            Type[] scripts = new Type[] { typeof(BarMagnet) };
            //to add a script just replace YourBlockScript below, add more through adding a "," and then write typeof(AnotherScript)
            //Type[] scripts = new Type[] { typeof(YourBlockScript) };
            #endregion

            //Parameter explanation: ID,     objName,       textureName,  blockName, icon, properties,        collider,       resources, mass, offset, scripts, show collider
            BlockLoader.AddModBlock(0xC7,"BarMagnet.obj","BarMagnet.png","BarMagnet",icon,blockProp,compoundCollider,neededResources,0.5f,offset,scripts,false);
        }

        public override void OnUnload() { }

        public static List<BarMagnet> barMagnetList = new List<BarMagnet>();

        public static void ResetList(bool isSimulating) {
            if(!isSimulating)
                barMagnetList = new List<BarMagnet>();
        }

        public static int AddToList(BarMagnet barMagnet) {
            int ret = barMagnetList.Count;
            barMagnetList.Add(barMagnet);
            return ret;
        }

    }

    public class BarMagnet : MonoBehaviour {
        int myID = -1;
        bool inverted = false;
        public static float maxDistance = 100f;
        public static float forceMultiplier = 200;
        public Vector3 pos1;
        public Vector3 pos2;
        public Rigidbody rigidBody;

        public void Start() {
            if(AddPiece.isSimulating) {
                myID = BarMagnetLoader.AddToList(this);
                rigidBody = GetComponent<Rigidbody>();
                inverted = GetComponent<MyBlockInfo>().toggleModeEnabled;
            }
        }

        public void FixedUpdate() {
            if(inverted){
                pos1 = transform.position + transform.forward*2;
                pos2 = transform.position;
            }
            else{
                pos2 = transform.position + transform.forward*2;
                pos1 = transform.position;
            }
            if(myID!=-1) {
                for(int i = 0;i<myID;i++) {
                    CalculateForce(BarMagnetLoader.barMagnetList[i]);
                }
            }
        }

        public void CalculateForce(BarMagnet barMagnet) {
            if(Vector3.Distance(transform.position+transform.forward,barMagnet.transform.position+barMagnet.transform.forward)>maxDistance)
                return;
            if(pos1==barMagnet.pos2||pos2==barMagnet.pos1) {
                return;
            }
            if(Vector3.Distance(pos1,barMagnet.pos2)<0.3f) {
                barMagnet.rigidBody.AddForceAtPosition((pos1-barMagnet.pos2).normalized,barMagnet.pos2);
                rigidBody.AddForceAtPosition((barMagnet.pos2-pos1).normalized,pos1);
                return;
            }
            if(Vector3.Distance(pos2,barMagnet.pos1)<0.3f) {
                barMagnet.rigidBody.AddForceAtPosition((pos2-barMagnet.pos1).normalized,barMagnet.pos1);
                rigidBody.AddForceAtPosition((barMagnet.pos1-pos2).normalized,pos2);
                return;
            }
            Vector3 Pp, Pn, Np, Nn;
            Pp = barMagnet.pos1-pos1;
            Pn = barMagnet.pos2-pos1;
            Np = barMagnet.pos1-pos2;
            Nn = barMagnet.pos2-pos2;
            Pp = Pp.normalized/(Vector3.SqrMagnitude(Pp));
            Pn = Pn.normalized/(Vector3.SqrMagnitude(Pn));
            Np = Np.normalized/(Vector3.SqrMagnitude(Np));
            Nn = Nn.normalized/(Vector3.SqrMagnitude(Nn));

            if(Pp.magnitude>100f) {
                Pp/= Pp.magnitude/100f;
            }
            if(Pn.magnitude>100f) {
                Pn/= Pn.magnitude/100f;
            }
            if(Np.magnitude>100f) {
                Np/= Np.magnitude/100f;
            }
            if(Nn.magnitude>100f) {
                Nn /= Nn.magnitude/100f;
            }
            barMagnet.rigidBody.AddForceAtPosition((Pp-Np)*forceMultiplier,barMagnet.pos1);
            barMagnet.rigidBody.AddForceAtPosition((Nn-Pn)*forceMultiplier,barMagnet.pos2);
            rigidBody.AddForceAtPosition((Pn-Pp)*forceMultiplier,pos1);
            rigidBody.AddForceAtPosition((Np-Nn)*forceMultiplier,pos2);
        }
    }

}