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(); }