Unity3D Ray OBB

using UnityEngine;
using System.Collections;

public class RaycastOBB : MonoBehaviour {
  public Vector3 rayOrigin = new Vector3(5.0f, 10.0f, 7.0f);
	public Vector3 rayDirection = new Vector3(-15.0f, 33.0f, 10.0f);
	public Vector3 obbRotation = new Vector3(10.0f, 260.0f, 170.0f);
	public Vector3 obbPosition = new Vector3(0.0f, 20.0f, 10.0f);
	public Vector3 obbHalfSize = new Vector3(1.0f, 1.0f, 1.0f);
	
	protected Matrix4x4 positionedMatrix = new Matrix4x4();
	protected Quaternion rotationQuat = Quaternion.identity;
	protected GameObject cube;
	protected GameObject sphere;
	
	void Awake() {
		rotationQuat = Quaternion.Euler(obbRotation);
		positionedMatrix.SetTRS(obbPosition, rotationQuat, Vector3.one);
		
		cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
		cube.name = "Visualize OBB";
		cube.transform.position = obbPosition;
		cube.transform.localRotation = rotationQuat;	
		cube.transform.localScale = obbHalfSize;
		
		sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
		sphere.name = "Visualize Original Point";
		sphere.transform.position = rayOrigin;
		sphere.transform.localScale = Vector3.one * 0.25f;
		
		// To move a point into OBB space, multiply it by the obb's inverse matrix (Position & rotation)
		// This will place the obb at origin (0, 0, 0) with it's up vector pointint up
	}
	
	void UpdateGameObjectPositions() {
		rotationQuat = Quaternion.Euler(obbRotation);
		positionedMatrix.SetTRS(obbPosition, rotationQuat, Vector3.one);
		
		cube.transform.position = obbPosition;
		cube.transform.localRotation = rotationQuat;	
		cube.transform.localScale = obbHalfSize;
		
		sphere.transform.position = rayOrigin;
		sphere.transform.localScale = Vector3.one * 0.25f;
	}
	
	void Update() {
		UpdateGameObjectPositions();
		
		Debug.DrawRay(rayOrigin, rayDirection.normalized, Color.blue);
		
		Vector3 hit, normal;
		Vector3 min = (obbHalfSize * -0.5f); // Box at origin
		Vector3 max = (obbHalfSize *  0.5f); // Box at origin
		
		// Get ray origin, in OBB space
		Vector3 origin = positionedMatrix.inverse.MultiplyPoint(rayOrigin);
		// Get ray direction in OBB space
		Vector3 direction = positionedMatrix.inverse.MultiplyVector(rayDirection).normalized;
		
		if (RayAABB(origin, direction, min, max, out hit, out normal)) {
			// Translate hit info back into worls space, and draw
			hit = positionedMatrix.MultiplyPoint(hit);
			normal = positionedMatrix.MultiplyVector(normal);
			
			float rayLength = (hit - rayOrigin).magnitude * 0.25f;
			Debug.DrawLine(hit, hit + normal * rayLength, Color.red); // This line should overlap the original blue line!
		}
	}
	
	// http://www.mpi-inf.mpg.de/departments/d4/teaching/ws200708/cg/rc/marold/src/AABB.html
	bool RayAABB(Vector3 /*ray*/origin, Vector3 /*ray*/direction, Vector3 min, Vector3 max, out Vector3 hitPosition, out Vector3 hitNormal) {
		Vector3 AABBCenter = (min + max) * 0.5f;
		direction = direction.normalized;
		hitNormal = Vector3.one.normalized;
		hitPosition = Vector3.zero;
		
		float tmin, tmax, tymin, tymax, tzmin, tzmax;
		Vector3 invrd = direction;
		invrd.x = 1.0f / invrd.x;
		invrd.y = 1.0f / invrd.y;
		invrd.z = 1.0f / invrd.z;
		
		if (invrd.x >= 0.0f) {
			tmin = (min.x - origin.x) * invrd.x;
			tmax = (max.x - origin.x) * invrd.x;
		} else {
			tmin = (max.x - origin.x) * invrd.x;
			tmax = (min.x - origin.x) * invrd.x;
		}
		
		if (invrd.y >= 0.0f) {
			tymin = (min.y - origin.y) * invrd.y;
			tymax = (max.y - origin.y) * invrd.y;
		} else {
			tymin = (max.y - origin.y) * invrd.y;
			tymax = (min.y - origin.y) * invrd.y;
		}
		
		if ((tmin > tymax) || (tymin > tmax)) {
			DrawDebugRay(origin, direction);
			return false;
		}
		if (tymin > tmin) tmin = tymin;
		if (tymax < tmax) tmax = tymax;
		
		if (invrd.z >= 0.0f) {
			tzmin = (min.z - origin.z) * invrd.z;
			tzmax = (max.z - origin.z) * invrd.z;
		} else {
			tzmin = (max.z - origin.z) * invrd.z;
			tzmax = (min.z - origin.z) * invrd.z;
		}
		
		if ((tmin > tzmax) || (tzmin > tmax)) {
			DrawDebugRay(origin, direction);
			return false;
		}
		if (tzmin > tmin) tmin = tzmin;
		if (tzmax < tmax) tmax = tzmax;
		
		if (tmin < 0) tmin = tmax;
		if (tmax < 0) {
			DrawDebugRay(origin, direction);
			return false;
		}
		
		float t = tmin;
		hitPosition = origin + t * direction;
		Vector3 dir = hitPosition - AABBCenter;
		
		Vector3 width = max - min;
		width.x = Mathf.Abs(width.x);
		width.y = Mathf.Abs(width.y);
		width.z = Mathf.Abs(width.z);
		
		Vector3 ratio = Vector3.one;
		ratio.x = Mathf.Abs(dir.x / width.x);
		ratio.y = Mathf.Abs(dir.y / width.y);
		ratio.z = Mathf.Abs(dir.z / width.z);
		
		hitNormal = Vector3.zero;
		int maxDir = 0; // x
		if (ratio.x >= ratio.y && ratio.x >= ratio.z) { // x is the greatest	
			maxDir = 0;
		} else if (ratio.y >= ratio.x && ratio.y >= ratio.z) { // y is the greatest	
			maxDir = 1;
		} else if (ratio.z >= ratio.x && ratio.z >= ratio.y) { // z is the greatest
			maxDir = 2;
		}
		
		if (dir[maxDir] > 0)
			hitNormal[maxDir] = 1.0f;
		else hitNormal[maxDir] = -1.0f;
		
		// Debug visualization
		float raySize = (positionedMatrix.MultiplyPoint(hitPosition) - positionedMatrix.MultiplyPoint(origin)).magnitude;
		Debug.DrawLine(rayOrigin, rayOrigin + rayDirection.normalized * raySize, Color.blue);
		DrawDebugRay(origin, direction, true);
		
		return true;
	}
	
	void DrawDebugRay(Vector3 origin, Vector3 direction, bool hit = false) {
		// Put the ray back into world space to draw debug lines
		Vector3 wRay = positionedMatrix.MultiplyPoint(origin);
		Vector3 wDir = positionedMatrix.MultiplyVector(direction).normalized;
		if (hit) {
			Debug.DrawLine(wRay, wRay + wDir * 3.0f, Color.yellow);
			Debug.DrawLine(wRay + wDir * 0.5f, wRay + wDir * 3.0f,Color.green);
		} else {
			Debug.DrawLine(wRay, wRay + wDir * 3.0f, Color.yellow); // Origin is gray!
			Debug.DrawLine(sphere.transform.position, cube.transform.position,Color.black);
		}
	}
}