/*
 *  CollisionDetector.cpp
 *  Martial
 *
 *  Created by ryoma on 10/01/26.
 *  Copyright 2010 琉球大学. All rights reserved.
 *
 */

#include "CollisionDetector.h"

void CollisionDetector::addCollisionObject(CollisionObject* cobj) {
	collisionObjectList.push_back(cobj);
}

void CollisionDetector::detectCollision(CollisionNode* cn1, CollisionNode* cn2) {
	cn1->setCollision(true);
	cn2->setCollision(true);
}

void CollisionDetector::checkCollision(CollisionObject* co1, CollisionObject* co2) {
	std::list<CollisionNode*> collisionNodeList1, collisionNodeList2;
	collisionNodeList1 = co1->getCollisionNodeList();
	collisionNodeList2 = co2->getCollisionNodeList();
	osg::NodePath npl1 , npl2;
	osg::Matrix m1, m2;
	std::list<CollisionNode*>::iterator node1, node2;
	for(node1 = collisionNodeList1.begin(); node1 != collisionNodeList1.end(); ++node1) {
		for(node2 = collisionNodeList2.begin(); node2 != collisionNodeList2.end(); ++node2) {
			npl1 = (*node1)->getNodePath();
			npl2 = (*node2)->getNodePath();
			//衝突判定
			m1 = osg::computeLocalToWorld(npl1);
			m2 = osg::computeLocalToWorld(npl2);
			if ((*node1)->getBound(m1).intersects((*node2)->getBound(m2))) {
				if (Martial::TEST::SHOW_COLLISION) printf("collision!! %s->[%s] & %s->[%s]\n", co1->getName(), (*node1)->getName(),
					co2->getName(), (*node2)->getName());
				detectCollision(*node1, *node2);
			}
		}
	}
}

void CollisionDetector::frame() {
	std::list<CollisionObject*>::iterator iter1, iter2;
	iter1 = collisionObjectList.begin();
	iter2 = iter1;
	float length;
	osg::Vec3 v1, v2;

	for(++iter1; iter1 != collisionObjectList.end(); ++iter1) {
		length = ((*iter1)->getPos()-(*iter2)->getPos()).length();
		if (length <= 3.0) {
			if (Martial::TEST::SHOW_COLLISION) printf("%s close to %s (%.3f), check collision..\n", (*iter2)->getName(), (*iter1)->getName(), length);
			checkCollision(*iter1, *iter2);
		}
		iter2 = iter1;
	}
}
/*
		startFrameTick = osg::Timer::instance()->tick();
		
		//SceneGraphの走査 -> 座標計算, 衝突判定
		//衝突処理はcheckCollision内部で行なう?(未実装)
		for(mobIter = movableObjectList.begin(); mobIter != movableObjectList.end(); ++mobIter) {
			(*mobIter)->reload();
			//このcheckCollisionは、後に削除
			//CollisionDetectorクラスを定義し、そこでcheckCollsionすることに
			(*mobIter)->checkCollision();
		}
*/