Move rigidbody2D with AddForceAtPosition relative to mouseclick (no snapping)

Hi.
I need help on figuring out how to move a rigidbody2D with force from mouse position, not from it’s centre of mass.

Here’s a quick video how my script currently works
[1]: https://docs.google.com/file/d/0B0ozmyp9_wwiMmN0bGg3XzhvMW8/edit?pli=1
[Link to video][1]

If you watch the video then you can see that if I click on the edges of the red cube then it want’s to align it’s centre with the mouse cursor. But I want to be able to drag the cube from the point that I clicked on it.
I tried getting the distance from the mouse position to the cubes transform and using that offset, but I still can’t get it working.

Here’s my script

using UnityEngine;
using System.Collections;

public class ForceDrag : MonoBehaviour
{
	public float toVel = 2.5f;//toVel converts the distance remaining to the target velocity - if too low, the rigidbody slows down early and takes a long time to stop;
	public float maxVel = 15.0f;//max speed the rigidbody will reach when moving;
	public float maxForce = 100.0f;//maxForce limits the force applied to the rigidbody in order to avoid excessive acceleration (and instability);
	public float gain = 5f;// gain, sets the feedback amount: if too low, the rigidbody stops before the target point; if too high, it may overshoot and oscillate 


	private int layerMask = 1 << 8;
	private GameObject[]players;
	private Camera mainCamera;
	private RaycastHit2D hit;
	private Vector3 mousePos;
	private Vector3 mousePosOnMouseDown;
	private Vector3 clickOffset;

	private static GameObject movableObject = null;

	void Start(){
		mainCamera = Camera.main;
		players = GameObject.FindGameObjectsWithTag("Player");
	
	}

	void FixedUpdate()
	{
		if(Input.GetMouseButton(0)){
			moveObject();
		}
	}

	void Update ()
		
	{
		mousePos = mainCamera.ScreenToWorldPoint(Input.mousePosition);
		mousePos.z=0;

		if (Input.GetMouseButtonDown (0)){

			foreach(GameObject player in players){
				player.rigidbody2D.isKinematic=false;//Allow all the chain links to move
			}

			assignMovableObject();
		
		}

		if (Input.GetMouseButtonUp(0)){
			movableObject=null;
			foreach(GameObject player in players){
				player.rigidbody2D.velocity=Vector2.zero;//Zero out linear velocity
				player.rigidbody2D.angularVelocity=0;//Zero out angular velocity
				player.rigidbody2D.isKinematic=true;//Stop all the chain links from moving
			}

		}
		
		
	}
	void assignMovableObject(){
		
		hit = Physics2D.Raycast(mainCamera.ScreenToWorldPoint (Input.mousePosition), Vector2.zero, Mathf.Infinity, layerMask);

		if (hit.collider && hit.rigidbody.isKinematic == false) {
			movableObject = hit.collider.gameObject;//Assign the object to be moved/dragged
		}
	
	}
	void calculateClickOffset(){
		if(movableObject){
			mousePosOnMouseDown = new Vector3(mousePos.x,mousePos.y,-10);
			clickOffset = mousePosOnMouseDown-movableObject.transform.position;//Distance from movable object to mousePos
		}
		
	}

	void moveObject(){
		if(movableObject){

			Vector2 dist = mousePos-movableObject.transform.position;
			// calc a target vel proportional to distance (clamped to maxVel)
			Vector2 tgtVel = Vector2.ClampMagnitude(toVel * dist, maxVel);
			// calculate the velocity error
			Vector2 error = tgtVel - movableObject.rigidbody2D.velocity;
			// calc a force proportional to the error (clamped to maxForce)
			Vector2 force = Vector2.ClampMagnitude(gain * error, maxForce);

			movableObject.rigidbody2D.AddForceAtPosition(force,mousePos);
		}
	}
	

}

I guess the Important part is in the moveObject function. My idea was to add the click offset to the calculation that gives me the dist variable, but I couldn’t find a way to get it working.
About the gameObject’s, they are simple planes that have rigidbody2D , boxCollider2D, hingeJoint2D. White cube is kinematic.

So after a month started working on this problem again and I think I got it working reasonably well.
Ended up using a PID controller. Most of the credit goes to aldonaletto. He’s answers were very useful.

Anyway here’s how it work’s with a single rigidbody2D
https://drive.google.com/file/d/0B0ozmyp9_wwiVkEwRUVsaVpIdE0/edit?usp=sharing
No snapping :slight_smile:

And here’s how it work’s with three rigidbody2D’s that are connected through hinge joint’s
https://drive.google.com/file/d/0B0ozmyp9_wwiSjZGMEx1Nk9VM1k/edit?usp=sharing
There’s still a little bit of snapping but this seems to be due to the connected hingejoints.

Here’s the code

using UnityEngine;
using System.Collections;

public class PIDAForceDrag : MonoBehaviour
{
	Vector2 targetPos = Vector2.zero; // the desired position
	public float maxForce = 100f; // the max force available
	public float pGain = 20f; // the proportional gain
	public float iGain = 0.5f; // the integral gain
	public float dGain = 0.5f; // differential gain
	private Vector2 integrator = Vector2.zero; // error accumulator
	private Vector2 lastError = Vector2.zero; 
	private Vector2 curPos = Vector2.zero; // actual Pos
	private Vector2 force = Vector2.zero; // current force

	private int layerMask = 1 << 8;
	private GameObject[]players;
	private Camera mainCamera;
	private RaycastHit2D hit;
	private Vector3 mousePos;
	private Vector3 mousePosOnMouseDown;
	private Vector3 clickOffset;

	private static GameObject movableObject = null;

	void Start(){
		mainCamera = Camera.main;
		players = GameObject.FindGameObjectsWithTag("Player");
	
	}

	void FixedUpdate()
	{
		if(Input.GetMouseButton(0)){
			moveObject();
		}
	}

	void Update ()
		
	{
		mousePos = mainCamera.ScreenToWorldPoint(Input.mousePosition);
		mousePos.z=0;

		if (Input.GetMouseButtonDown (0)){
			


			foreach(GameObject player in players){
				player.rigidbody2D.isKinematic=false;//Allow all the chain links to move
			}

			assignMovableObject();
			calculateClickOffset();
		
		}

		if (Input.GetMouseButtonUp(0)){
			movableObject=null;
			foreach(GameObject player in players){
				player.rigidbody2D.velocity=Vector2.zero;//Zero out linear velocity
				player.rigidbody2D.angularVelocity=0;//Zero out angular velocity
				player.rigidbody2D.isKinematic=true;//Stop all the chain links from moving
			}

		}
		
		
	}
	void assignMovableObject(){
		
		hit = Physics2D.Raycast(mainCamera.ScreenToWorldPoint (Input.mousePosition), Vector2.zero, Mathf.Infinity, layerMask);

		if (hit.collider && hit.rigidbody.isKinematic == false) {
			movableObject = hit.collider.gameObject;//Assign the object to be moved/dragged
		}
	
	}
	void calculateClickOffset(){

		if(movableObject){
			mousePosOnMouseDown = new Vector3(mousePos.x,mousePos.y,-10);
			clickOffset = mousePosOnMouseDown-movableObject.transform.position;//Distance from movable object to mousePos

		}
		
	}

	void moveObject(){
		if(movableObject){
			targetPos=mousePos-clickOffset;
			curPos = movableObject.transform.position;
			Vector2 error = targetPos - curPos; // generate the error signal
			integrator += error * Time.deltaTime; // integrate error
			Vector2 diff = (error - lastError)/ Time.deltaTime; // differentiate error
			lastError = error;
			// calculate the force summing the 3 errors with respective gains:
			force = error * pGain + integrator * iGain + diff * dGain;
			// clamp the force to the max value available
			force = Vector3.ClampMagnitude(force, maxForce);

			// apply the force to accelerate the rigidbody:
			//movableObject.rigidbody2D.AddForce(force);


			movableObject.rigidbody2D.AddForce(force);
			//movableObject.rigidbody2D.AddForceAtPosition(force,mousePos);
		}
	}
	

}

I ended up leaving IGain to 0 as it worked best for me.