void FixedUpdate()
{
// Retreive positions and velocity of handle and proxy
Vector3 posHandle = this.sizeFactor * (socket.GetPosition());
Vector3 velHandle = this.sizeFactor * (socket.GetSpeed());
Vector3 posProxy = this.rb.position;
Vector3 velProxy = this.rb.velocity;
// Copy the position of the haptic interface to the proxy
handle.transform.position = posHandle;
// Compute force applied to proxy and haptic interface (spring-dampener equations)
this.deltaPos = posHandle - posProxy;
this.force = this.k * deltaPos - this.b * (velProxy - velHandle);
/*Vector3 magForce = getMagneticForce(posProxy);
* print(magForce);*/
// Applying forces to proxy
this.rb.AddForce(force);
if (this.handleScript.GetStatus()) // Applies a force if handle is colliding with an object
{
this.socket.SetForce(-(force));
}
}
//Vector3 B = new Vector3(1/8, 1/8, 1/8);
// Use this for initialization
void Start()
{
this.socket = ScriptableObject.CreateInstance <SocketImpedance>();
this.force = Vector3.zero; // force vector to be sent to haptic interface
// Getting proxy Rigidbody and its parameters
this.rb = GetComponent <Rigidbody>();
this.m = this.rb.mass;
// Computing dampening (stability)
this.b = 2f * (float)Math.Pow(this.k * this.m, 0.5f);
print("INIT:: Coefficients: k = " + this.k + ", b = " + this.b);
// Getting Handle GameObject and its script
handle = GameObject.Find("Handle");
handleScript = this.handle.GetComponent <HandleManager>();
// Initialize position of handle and proxy to avoid huge forces
// at the start of the game
this.rb.position = this.sizeFactor * (socket.GetPosition());
handle.transform.position = this.sizeFactor * (socket.GetPosition());
print("INIT:: Haptics Done");
}