void CameraController::rotateTowardRobot(Vector3d r_pos)
{
m_my->setPosition(m_rotatePos);
// カメラの位置を得る
m_my->getPosition(m_pos);
// カメラの位置からロボットを結ぶベクトル
Vector3d tmpp = r_pos;
tmpp -= m_pos;
// y方向は考えない
tmpp.y(0);
// カメラの回転角度を得る
Rotation myRot;
m_my->getRotation(myRot);
// カメラのy軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// ロボットまでの回転角度を得る
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
// 角度差から回転量を得る
targetAngle += theta;
m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0);
}
std::string RobotController::getPointedTrashName(std::string entName)
{
// 発話者の名前からSimObjを取得します
SimObj *tobj = getObj(entName.c_str());
// メッセージ送信者の左肘関節の位置を取得します
Vector3d jpos;
if(!tobj->getJointPosition(jpos, "RARM_JOINT4")) {
LOG_ERR(("failed to get joint position"));
return "";
}
// メッセージ送信者の左肘から左手首をつなぐベクトルを取得します
Vector3d jvec;
if(!tobj->getPointingVector(jvec, "RARM_JOINT4", "RARM_JOINT7")) {
LOG_ERR(("failed to get pointing vector"));
return "";
}
double distance = 0.0;
std::string objName = "";
// 全ゴミオブジェクトでループします
int trashboxSize = m_trashboxs.size();
for(int i = 0; i < trashboxSize; i++) {
// エンティティの位置を取得します
SimObj *obj = getObj(m_trashboxs[i].c_str());
Vector3d objVec;
obj->getPosition(objVec);
// エンティティと左肘関節を結ぶベクトルを作成します
objVec -= jpos;
// cos角度が不の場合（指差した方向と反対側にある場合)は対象から外します
double cos = jvec.angle(objVec);
if(cos < 0)
continue;
// 指差した方向ベクトルまでの最短距離の計算
double theta = acos(cos);
double tmp_distance = sin(theta) * objVec.length();
// 最小距離の場合は名前、距離を保存しておく
if(tmp_distance < distance || distance == 0.0){
distance = tmp_distance;
objName = obj->name();
}
}
// エンティティでループして最も近いオブジェクトの名前を取得する
return objName;
}
double MyController::rotateTowardObj(Vector3d pos)
{
// 自分の位置の取得
Vector3d myPos;
my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
return targetAngle;
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
if(targetAngle == 0.0){
return 0.0;
}
else {
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 回転時間(u秒)
double time = distance / vel;
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
double MyController::rotateTowardGrabPos(Vector3d pos, double velocity, double now)
{
printf("start rotate %lf \n", now);
//自分を取得
SimObj *my = getObj(myname());
//printf("向く座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
//自分の手のパーツを得ます
CParts * parts = my->getParts("RARM_LINK7");
Vector3d partPos;
parts->getPosition(partPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
//tmpp -= myPos;
tmpp -= partPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
//printf("qw: %lf theta: %lf \n", qw, theta);
if(qw*qy < 0) {
//printf("qw * qy < 0 \n");
theta = -1*theta;
}
// z方向からの角度
//printf("結ぶベクトル 座標 %lf %lf %lf \n", tmpp.x(), tmpp.y(), tmpp.z());
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
//printf("tmp: %lf \n", tmp);
double targetAngle = acos(tmp);
//printf("targetAngle: %lf ---> %lf \n", targetAngle, targetAngle*180.0/PI);
// 方向
//printf("tmpp.x() %lf \n", tmpp.x());
if(tmpp.x() > 0) {
targetAngle = -1*targetAngle;
//printf("targetAngle: %lf deg \n", targetAngle*180.0/PI);
}
targetAngle += theta;
//printf("targetAngle: %lf <--- %lf \n", targetAngle, theta);
//printf("qw: %lf qy: %lf theta: %lf tmp: %lf targetAngle: %lf \n", qw, qy, theta, tmp, targetAngle);
if(targetAngle == 0.0){
//printf("donot need rotate \n");
return 0.0;
}
else
{
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 回転時間(u秒)
double time = distance / vel;
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else
{
m_my->setWheelVelocity(-velocity, velocity);
}
//printf("distance: %lf vel: %lf time: %lf \n", distance, vel, time);
printf("rotate time: %lf, time to stop: %lf \n", time, now + time);
return now + time;
}
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// get own position
Vector3d myPos;
m_my->getPosition(myPos);
// vector from own position to a target position
Vector3d tmpp = pos;
tmpp -= myPos;
// rotation about y-axis is always 0
tmpp.y(0);
// get own rotation
Rotation myRot;
m_my->getRotation(myRot);
// initial direction
Vector3d iniVec(0.0, 0.0, 1.0);
// get rotation angle about y-axis
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0){
theta = -theta;
}
// angle from z-axis
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// calcurate target angle
if(tmpp.x() > 0){
targetAngle = -1*targetAngle;
}
targetAngle += theta;
if(targetAngle<-M_PI){
targetAngle += 2*M_PI;
}
else if(targetAngle>M_PI){
targetAngle -= 2*M_PI;
}
if(targetAngle == 0.0){
return 0.0;
}
else {
// circumference length for rotation
double distance = m_distance*M_PI*fabs(targetAngle)/(2*M_PI);
// calcurate velocity from radius of wheels
double vel = m_radius*velocity;
// rotation time (micro second)
double time = distance / vel;
// start rotating
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI);
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI);
if(targetAngle == 0.0){
return 0.0;
}
else {
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
printf("distance: %lf \n", distance);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
printf("radius: %lf, velocity: %lf, vel: %lf \n", m_radius, velocity, vel);
// 回転時間(u秒)
double time = distance / vel;
printf("rotateTime: %lf = dis: %lf / vel: %lf\n", time, distance, vel);
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
void motionCallback(int x, int y)
{
if (selectedView == FRONT)
{
controlPoints[selectedPoint[0]][selectedPoint[1]][0] = ToLocalX(x);
controlPoints[selectedPoint[0]][selectedPoint[1]][1] = ToLocalY(y - height/2);
}
else if (selectedView == UP)
{
controlPoints[selectedPoint[0]][selectedPoint[1]][0] = ToLocalX(x);
controlPoints[selectedPoint[0]][selectedPoint[1]][2] = -ToLocalY(y);
}
else if (selectedView == LEFT)
{
controlPoints[selectedPoint[0]][selectedPoint[1]][2] = -ToLocalX(x - width/2);
controlPoints[selectedPoint[0]][selectedPoint[1]][1] = ToLocalY(y - height/2);
}
else if (selectedView == ROTATE)
{
Vector3d lastP = getMousePoint(lastX, lastY, width / 2.0f, height / 2.0f, radius);
Vector3d currentP = getMousePoint(x - width / 2.0f, y, width / 2.0f, height / 2.0f, radius);
if (mouseButton == GLUT_LEFT_BUTTON)
{
Vector3d rotateVector;
rotateVector.cross(currentP, lastP);
double angle = -currentP.angle(lastP) * 2;
rotateVector = unProjectToEye(rotateVector, eye, center, upVector);
Vector3d dEye;
dEye.sub(center, eye);
dEye = rotate(dEye, rotateVector, -angle);
upVector = rotate(upVector, rotateVector, -angle);
eye.sub(center, dEye);
}
else if (mouseButton == GLUT_RIGHT_BUTTON) {
Vector3d dEye;
dEye.sub(center, eye);
double offset = 0.025;
if ((y - lastY) < 0) {
dEye.scale(1 - offset);
}
else {
dEye.scale(1 + offset);
}
eye.sub(center, dEye);
}
else if (mouseButton == GLUT_MIDDLE_BUTTON) {
double dx = x - width / 2.0f - lastX;
double dy = y - lastY;
if (dx != 0 || dy != 0)
{
Vector3d moveVector(dx, dy, 0);
moveVector = unProjectToEye(moveVector, eye, center, upVector);
moveVector.normalize();
double eyeDistance = Vector3d(eye).distance(Vector3d(center));
moveVector.scale(std::sqrt(dx*dx + dy*dy) / 1000 * eyeDistance);
center.add(moveVector);
eye.add(moveVector);
}
}
lastX = x - width / 2;
lastY = y;
}
EvaluateSurface();
glutPostRedisplay();
}