Mercurial > hg > Members > e075743

view src/Robot.cpp @ 11:4727e16018cd

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
change src. punch motion using array
author tokumoritaichirou@w-133-13-243-110.cc.u-ryukyu.ac.jp
date Wed, 03 Feb 2010 12:02:57 +0900
parents 5727d511a13a
children d45a95c697c3
line wrap: on
line source

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

#include "Robot.h"

Robot::Robot(char* _name) : Humanoid::Humanoid(_name) {
	moveCount = 0.0;
	scale = osg::Vec3(0.01,0.01,0.01);

	humanoidNode = osgDB::readNodeFile("robot.osg");
	humanoidForm = new osg::PositionAttitudeTransform();
	humanoidForm->addChild(humanoidNode);
	mform->addChild(humanoidForm);

	//ノードにアクセス。MatrixTransformをnameで検索(visitor内部で std::map で保持)
	SearchMatrixTransformVisitor visitor = SearchMatrixTransformVisitor();

	humanoidNode->accept(visitor);
	bodyForm = visitor.getMatrixTransform("body");
	leftUpperArmForm = visitor.getMatrixTransform("left_arm1");
	rightUpperArmForm = visitor.getMatrixTransform("right_arm1");
	leftDownArmForm = visitor.getMatrixTransform("left_arm2");
	rightDownArmForm = visitor.getMatrixTransform("right_arm2");
	leftUpperLegForm = visitor.getMatrixTransform("left_leg1");	
	rightUpperLegForm = visitor.getMatrixTransform("right_leg1");
	leftDownLegForm = visitor.getMatrixTransform("left_leg2");	
	rightDownLegForm = visitor.getMatrixTransform("right_leg2");
	
	//衝突判定ノードの追加
	CollisionNode* collisionNode;

	bodyForm->setMatrix(osg::Matrix());

	collisionNode = new CollisionNode("Body-Node1", new osg::Sphere(osg::Vec3(0, 0, 0), 50));
	//bodyForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);
	collisionNode = new CollisionNode("Body-Node2", new osg::Sphere(osg::Vec3(0, 0, 60), 50));
	//bodyForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);
	collisionNode = new CollisionNode("Body-Node3", new osg::Sphere(osg::Vec3(0, 0, -60), 50));
	//bodyForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);
	collisionNode = new CollisionNode("Left-Arm-Node", new osg::Sphere(osg::Vec3(100, -40, -70), 30));
	//leftUpperArmForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);
	collisionNode = new CollisionNode("Right-Arm-Node", new osg::Sphere(osg::Vec3(-100, -40, -70), 30));
	//rightUpperArmForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);
	collisionNode = new CollisionNode("Left-Leg-Node", new osg::Sphere(osg::Vec3(0, 0, 0), 30));
	//leftUpperLegForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);
	collisionNode = new CollisionNode("Right-Leg-Node", new osg::Sphere(osg::Vec3(0, 0, 0), 30));
	//rightUpperLegForm->addChild(collisionNode->getGeode());
	collisionNodeList.push_back(collisionNode);	

	SearchGeodeVisitor gvisitor = SearchGeodeVisitor();

	gvisitor.setSearchGeodeName("Collision_right_leg1_geode");
	humanoidNode->accept(gvisitor);

	//std::cout << "getVisitor: result = " << gvisitor.getGeode().getName(); << "\n";

	unWaitFunc = &Robot::nop;
	funcs[PUSH_LEFT]		= &Robot::walk;
	funcs[PUSH_RIGHT]		= &Robot::turn;
	funcs[PUSH_UP]			= &Robot::jump;
	funcs[PUSH_DOWN]		= &Robot::squat;
	funcs[PUSH_RIGHTUP]		= &Robot::jump;
	funcs[PUSH_LEFTUP]		= &Robot::jump;
	funcs[PUSH_RIGHTDOWN]	= &Robot::walk;
	funcs[PUSH_LEFTDOWN]	= &Robot::walk;	
	funcs[RELEASE_HAT]		= &Robot::stop;
	funcs[PUSH_A]			= &Robot::punch;
	funcs[PUSH_B]			= &Robot::nop;
	funcs[PUSH_C]			= &Robot::nop;
	funcs[PUSH_D]			= &Robot::nop;
	funcs[RELEASE_BUTTON]	= &Robot::stop;
}

void Robot::releaseHat() {
	printInput("Class Robot [%s] :: RELEASE-HAT\n", name);
	(this->*funcs[RELEASE_HAT])();
}

void Robot::pushLeft() {
	printInput("Class Robot [%s] :: PUSH-LEFT\n", name);
	(this->*funcs[PUSH_LEFT])();
}

void Robot::pushRight() {
	printInput("Class Robot [%s] :: PUSH-RIGHT\n", name);
	(this->*funcs[PUSH_RIGHT])();
}

void Robot::pushUp() {
	printInput("Class Robot [%s] :: PUSH-UP\n", name);
	(this->*funcs[PUSH_UP])();
}

void Robot::pushDown() {
	printInput("Class Robot [%s] :: PUSH-DOWN\n", name);
	//(this->*funcs[PUSH_RIGHTDOWN])();
	(this->*funcs[PUSH_A])();
}

void Robot::pushRightUp() {
	printInput("Class Robot [%s] :: PUSH-RIGHT-UP\n", name);
	(this->*funcs[PUSH_RIGHTUP])();
}
void Robot::pushRightDown() {
	printInput("Class Robot [%s] :: PUSH-RIGHT-DOWN\n", name);
	(this->*funcs[PUSH_RIGHTDOWN])();
}
void Robot::pushLeftUp() {
	printInput("Class Robot [%s] :: PUSH-LEFT-UP\n", name);
	(this->*funcs[PUSH_RIGHTUP])();
}

void Robot::pushLeftDown() {
	printInput("Class Robot [%s] :: PUSH-LEFT-DOWN\n", name);
	(this->*funcs[PUSH_LEFTDOWN])();
}

void Robot::pushA() {
	printInput("Class Robot [%s] :: PUSH-A\n", name);
	(this->*funcs[PUSH_A])();
}

void Robot::releaseA() {
	printInput("Class Robot [%s] :: RELEASE-A\n", name);
}

void Robot::pushB() {
	printInput("Class Robot [%s] :: PUSH-B\n", name);
	(this->*funcs[PUSH_B])();
}

void Robot::releaseB() {
	printInput("Class Robot [%s] :: RELASE-B\n", name);
}

void Robot::pushC() {
	printInput("Class Robot [%s] :: PUSH-C\n", name);
	(this->*funcs[PUSH_C])();
}

void Robot::releaseC() {
	printInput("Class Robot [%s] :: RELEASE-C\n", name);
}

void Robot::pushD() {
	printInput("Class Robot [%s] :: PUSH-D\n", name);
	(this->*funcs[PUSH_D])();
}

void Robot::releaseD() {
	printInput("Class Robot [%s] :: RELEASE-D\n", name);
}

void Robot::stackPush() {
	stack[PUSH_LEFT]		= funcs[PUSH_LEFT];
	stack[PUSH_RIGHT]		= funcs[PUSH_RIGHT];
	stack[PUSH_UP]			= funcs[PUSH_UP];
	stack[PUSH_DOWN]		= funcs[PUSH_DOWN];
	stack[PUSH_RIGHTUP]		= funcs[PUSH_RIGHTUP];
	stack[PUSH_LEFTUP]		= funcs[PUSH_LEFTUP];
	stack[PUSH_RIGHTDOWN]	= funcs[PUSH_RIGHTDOWN];
	stack[PUSH_LEFTDOWN]	= funcs[PUSH_LEFTDOWN];
	stack[RELEASE_HAT]		= funcs[RELEASE_HAT];
	stack[PUSH_A]			= funcs[PUSH_A];
	stack[PUSH_B]			= funcs[PUSH_B];
	stack[PUSH_C]			= funcs[PUSH_C];
	stack[PUSH_D]			= funcs[PUSH_D];
	stack[RELEASE_BUTTON]	= funcs[RELEASE_BUTTON];

	funcs[PUSH_LEFT]		= &Robot::wait;
	funcs[PUSH_RIGHT]		= &Robot::wait;
	funcs[PUSH_UP]			= &Robot::wait;
	funcs[PUSH_DOWN]		= &Robot::wait;
	funcs[PUSH_RIGHTUP]		= &Robot::wait;
	funcs[PUSH_LEFTUP]		= &Robot::wait;
	funcs[PUSH_RIGHTDOWN]	= &Robot::wait;
	funcs[PUSH_LEFTDOWN]	= &Robot::wait;
	funcs[RELEASE_HAT]		= &Robot::wait;
	funcs[PUSH_A]			= &Robot::wait;
	funcs[PUSH_B]			= &Robot::wait;
	funcs[PUSH_C]			= &Robot::wait;
	funcs[PUSH_D]			= &Robot::wait;
	funcs[RELEASE_BUTTON]	= &Robot::wait;
}

void Robot::stackPop() {
	funcs[PUSH_LEFT]		= stack[PUSH_LEFT];
	funcs[PUSH_RIGHT]		= stack[PUSH_RIGHT];
	funcs[PUSH_UP]		= stack[PUSH_UP];
	funcs[PUSH_DOWN]		= stack[PUSH_DOWN];
	funcs[PUSH_RIGHTUP]	= stack[PUSH_RIGHTUP];
	funcs[PUSH_LEFTUP]	= stack[PUSH_LEFTUP];
	funcs[PUSH_RIGHTDOWN]	= stack[PUSH_RIGHTDOWN];
	funcs[PUSH_LEFTDOWN]	= stack[PUSH_LEFTDOWN];
	funcs[RELEASE_HAT]	=	stack[RELEASE_HAT];
	funcs[PUSH_A]			= stack[PUSH_A];
	funcs[PUSH_B]			= stack[PUSH_B];
	funcs[PUSH_C]			= stack[PUSH_C];
	funcs[PUSH_D]			= stack[PUSH_D];
	funcs[RELEASE_BUTTON]	= stack[RELEASE_BUTTON];
}

/*  Robotの各モーションを定義 */

void Robot::punch() {
	float sinVal = sin(moveCount);
	float cosVal = cos(moveCount);
	double punch[6][3] = {{5.0,-30.0,-30.0},{10.0,-30.0,-90.0},{15.0,-120.0,60.0}};
	osg::Matrixd roteMatrix;
	osg::Matrixd dirMatrix;
	osg::Matrixd accMatrix;
	
	bodyForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(punch[(int)moveCount%3][0]), osg::Vec3(0,0,1)));
	rightUpperArmForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(punch[(int)moveCount%3][1]), osg::Vec3(1,0,0)));
	rightDownArmForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(punch[(int)moveCount%3][2]), osg::Vec3(1,0,0)));
	moveCount += 1;
	//if(moveCount <= 0.5){
		//bodyForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*10.0), osg::Vec3(0,0,1)));
		//rightUpperArmForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*60.0-90), osg::Vec3(1,0,0)));
		//rightDownArmForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*60.0-20), osg::Vec3(1,0,0)));
	//}
}

void Robot::walk() {
	velocity = osg::Vec3(0.5*avatarDirection,0,0);
	moveCount += (float)avatarDirection * 0.5;
	float sinVal = sin(moveCount);
	float cosVal = cos(moveCount);

	setPos(osg::Vec3(position.x(), position.y(), position.z()+cos(moveCount)*0.01));

	osg::Matrixd roteMatrix;
	osg::Matrixd dirMatrix;
	osg::Matrixd accMatrix;

	//positionAttitudeMatrix.setAttitude();での演算にくらべると、倍高速?
	bodyForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*10.0), osg::Vec3(0,0,1)));
	leftUpperArmForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*30.0+10), osg::Vec3(1,0,0)));
	//rightUpperArmForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*-30.0+10), osg::Vec3(1,0,0)));
	leftUpperLegForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*-45.0), osg::Vec3(1,0,0)));
	rightUpperLegForm->setMatrix(osg::Matrixf::rotate(osg::DegreesToRadians(sinVal*45.0), osg::Vec3(1,0,0)));
	
}

void Robot::turn() {
	angularVelocity = osg::Vec3(0,0,osg::DegreesToRadians(180.0/10.0));
	motionTime = 10;
	avatarDirection *= -1;
	void (Robot::*tmp)() = funcs[PUSH_LEFT];
	funcs[PUSH_LEFT] = funcs[PUSH_RIGHT];
	funcs[PUSH_RIGHT] = tmp;
	stackPush();
	unWaitFunc = &Robot::stop;
	wait();
}

void Robot::jump() {
	funcs[PUSH_UP] = &Robot::nop;
	funcs[PUSH_RIGHTUP] = &Robot::nop;
	funcs[PUSH_LEFTUP] = &Robot::nop;	
	velocity = osg::Vec3(velocity.x(), velocity.y(), 4);
	acceleration = osg::Vec3(acceleration.x(), acceleration.y(), -0.3);
}

void Robot::stop() {
	//moveCount = 0;
	motionTime = 0;
	velocity = osg::Vec3(0,0,0);
	angularVelocity = osg::Vec3(0,0,0);
}

void Robot::wait() {
	if ((--motionTime) < 0) {
		(this->*unWaitFunc)();
		unWaitFunc = &Robot::nop;
		stackPop();
	}
}

//以下、未実装

void Robot:: run() {};
void Robot:: squat() {};
void Robot:: down() {};
void Robot:: freeze() {};

void Robot::frame() {
	update();
}

void Robot::update() {

	//[当たり判定の処理] - 未実装,ただ判定を消すだけ
	std::list<CollisionNode*>::iterator iter;
	for (iter = collisionNodeList.begin(); iter != collisionNodeList.end(); ++iter) {
		(*iter)->setCollision(false);
	}

	osg::Matrixd roteMatrix;
	osg::Matrixd dirMatrix;
	osg::Matrixd accMatrix;

	direction += angularVelocity;
	velocity  += acceleration;

	accMatrix.makeTranslate(acceleration);
	roteMatrix.makeRotate(
		osg::Quat(
			direction.x(), osg::Vec3(1,0,0),
			direction.y(), osg::Vec3(0,1,0),
			direction.z(), osg::Vec3(0,0,1)
		)
	);

	velocity += (accMatrix*roteMatrix).getTrans();
	position += velocity;

	// 地面を抜けた場合
	if (position.z() < 0) {
		position.z() = 0;
		funcs[PUSH_UP] = &Robot::jump;
		funcs[PUSH_RIGHTUP] = &Robot::jump;
		funcs[PUSH_LEFTUP] = &Robot::jump;
		velocity = osg::Vec3(velocity.x(), velocity.y(), 0);
		acceleration = osg::Vec3(acceleration.x(), acceleration.y(), 0);
	}

	accMatrix.makeTranslate(acceleration);
	roteMatrix.makeRotate(
		osg::Quat(
			defaultDirection.x()+direction.x(), osg::Vec3(1,0,0),
			defaultDirection.y()+direction.y(), osg::Vec3(0,1,0),
			defaultDirection.z()+direction.z(), osg::Vec3(0,0,1)
		)
	);

	dirMatrix.makeTranslate(position);
	accMatrix.makeScale(scale);
	mform->setMatrix(accMatrix*roteMatrix*dirMatrix);
}