# Vector math question

I'm working on flocking behaviour AI, and it was a long time since I did any linear algebra, so I would be grateful from some input from anybody who perhaps has these things fresh in memory.

I have an sphere, about 0.5 units in size, which travels forward along its local coordinate system - it's always turned towards its target. Before the sphere is moved (it's not a rigidbody, btw), I cast another much larger sphere (about 2 units) to the point where its velocity would take it if applied as is. That's the ideal, unhindered movement.

If any collisions are detected, what I want to do is rotate the sphere away from the collision point, using the normal of the point of the hit. If the collision takes place very close to the sphere, the new rotation should be exactly that of the normal. The greater the distance, the less the normal should affect the current rotation of the sphere, down to 0 at the very edge of the larger sphere (or "discomfort zone"). This modification, done for all collisions, should result in smooth collision avoidance.

The problem is that I can't seem to get the rotation right. It's difficult to understand what the best practices are in this respect in Unity, especially since quaternions might be involved.

This is what I have so far (the script is attached to the sphere):

``````var target : Transform;
var speed : float = 1.0;
var comfortDist : float = 2.0;
var r : float;
var repulsionExtent : float;

function Start() {
r = transform.lossyScale.x;
repulsionExtent = comfortDist - r;
}

function Update() {
if (!target) return;
// Turn towards the target
transform.LookAt(target);

var pos : Vector3 = transform.position;
var dist : float = Time.deltaTime * speed;
var ray : Ray = new Ray(pos, target.position - pos);
var dir : Vector3 = transform.TransformDirection(Vector3.forward);
Debug.DrawRay(pos, dir * 10);

var hits : RaycastHit[] = Physics.SphereCastAll(ray, comfortDist, dist);
for (var h in hits) {
if (h.transform != target && h.collider != this.collider) {
var realDist : float = Vector3.Distance(pos, h.point) - r;

Debug.DrawLine(pos, h.point, Color.red);
Debug.DrawRay(h.point, h.normal, Color.blue);

var repulsion = (repulsionExtent - realDist) / repulsionExtent;
print("Repulsion: " + repulsion);

//var ftRot = Quaternion.FromToRotation(transform.eulerAngles, h.normal);

//transform.Rotate(h.normal * repulsion * 180);
//transform.rotation = Quaternion.LookRotation(transform.rotation + h.normal * repulsion * 180);
//transform.rotation = Quaternion.Slerp(transform.rotation, Quaternion.Euler(h.normal), repulsion * 180);
//transform.rotation = transform.rotation * h.normal * repulsion;

transform.rotation = Quaternion.RotateTowards(transform.rotation, Quaternion.Euler(h.normal),
repulsion * -45.0);

};
};

var newdir : Vector3 = transform.TransformDirection(Vector3.forward);
Debug.DrawRay(pos, newdir * 10, Color.yellow);

// Move forward
// transform.Translate(Vector3.forward * dist);
}

``````

As you can see, the actual movement is commented out at the moment. I have also included various non-working rotation attempts, also commented out. The variable "repulsion" spans from 0.0 to 1.0 and represents the amount that the present rotation should be modified by.

I might be working against the Unity API - there might be better ways of accomplishing what I want to do. I need to understand the correct procedure before I start implementing flocking and collision avoidance using potential functions, which would be the next step. The basic problem here is of course fundamental steering via vector rotation. (If I were using rigidbodies I probably would apply forces, which might be simpler.)

Any help most appreciated.

/ Peter

I know it's commented out, but this:

``````var ftRot = Quaternion.FromToRotation(transform.eulerAngles, h.normal);

``````

Is wrong. Quaternion.FromToRotation() takes two vectors as input, and Transform.eulerAngles is really a vector in name only (it's actually just a set of three angles, and has no meaning in the context of the 'from-to' rotation function).

If I recall correctly, the 'canonical' approach to steering behaviors is based on modifying the velocity vector; the orientation is then derived directly from the velocity vector (rather than being updated independently as in your code above).

Here's one way you could go about it though (based on your current implementation).

First, you'll need a parametric value indicating how much the agent's orientation should be effected. Assuming the agent is within the 'discomfort zone', this value can be computed as (pseudocode, untested):

``````float t = 1 - (distance / comfortDistance);

``````

You can then compute the desired direction as:

``````Vector3 direction = Vector3.Slerp(currentForward, normal, t);

``````

Then, use Quaternion.FromToRotation() to compute the quaternion that will rotate the current forward vector onto the desired direction vector, and apply it to Transform.rotation, e.g.:

``````transform.rotation =
Quaternion.FromToRotation(currentForward, direction) * transform.rotation;

``````

The above is all off the top of my head and is untested, but maybe it'll help point you in the right direction. (Note that you could also re-arrange things and use Quaternion.Slerp() instead; the results should be similar in either case.)