Алгоритм на препятсвия в 3D

Здравствуйте уважаемые форумчане.

Столкнунлся с такой проблемой: не знаю как реализовать алгоритм на препятствия в трехмерном пространстве. Допустим у меня есть загруженная модель дома. Как ограничить камеру в движении, чтобы она насквозь не проходила? Или же у меня есть лестница между этажами. Как подняться по ней?

Заранее спасибо.

Колизить камеру по axis aligned box

 while ( objects++ )
 movetrue = aabbTest(camera->getAABB(), model->getAABB );
 if ( movetrue ) camera.calcTransformation();