void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start, std::vector<geometry_msgs::PoseStamped>& path) { int n = path.size(); switch(omode_) { case FORWARD: for(int i=0;i<n-1;i++){ pointToNext(path, i); } break; case INTERPOLATE: path[0].pose.orientation = start.pose.orientation; interpolate(path, 0, n-1); break; case FORWARDTHENINTERPOLATE: for(int i=0;i<n-1;i++){ pointToNext(path, i); } int i=n-3; double last = getYaw(path[i]); while( i>0 ){ double new_angle = getYaw(path[i-1]); double diff = fabs(angles::shortest_angular_distance(new_angle, last)); if( diff>0.35) break; else i--; } path[0].pose.orientation = start.pose.orientation; interpolate(path, i, n-1); break; } }
void Rotate::enter() { autoLog->log("Entering state 'Rotate' and rotating from " + std::to_string(getYaw()) + " to " + std::to_string(getYaw() + dest) + " degrees..."); startRot = getYaw(); armUp(); Wait(ARMUP_PULSETIME); //need to raise the arm a bit before rotating armStop(); }
void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& path, int start_index, int end_index) { double start_yaw = getYaw(path[start_index]), end_yaw = getYaw(path[end_index ]); double diff = angles::shortest_angular_distance(start_yaw, end_yaw); double increment = diff/(end_index-start_index); for(int i=start_index; i<=end_index; i++){ double angle = start_yaw + increment * i; set_angle(&path[i], angle); } }
void Collectible::onPrepare( float dt ) { //update yaw setYaw(getYaw() + (SPIN_SPEED * dt)); //get bullet representation btMotionState *ms = getRigidBody()->getMotionState(); btTransform transform = getRigidBody()->getWorldTransform(); //update bullet representation btQuaternion q = btQuaternion(getYaw(), getPitch(), 0.0f); transform *= btTransform(q); ms->setWorldTransform(transform); }
void AIController::run(int frameRate) { Controller::run(frameRate); coord[pos][0] = shipCoord().getX(); coord[pos][1] = shipCoord().getY(); pos++; it++; pos %= LOG_SIZE; if (it >= 5) { if (coord[pos][0] < coord[(pos + 1) % LOG_SIZE][0] && shipCoord().getX() < 0 && shipCoord().getZ() < 0 && !rotated) { // std::cout << "*"; if (getYaw() <= MAX_ROTATE_POWER) { plusYaw(); } } else { rotateX(); moveX(); rotateY(); moveY(); moveForward(); shoot(); } } // std::cout << "coord: " << shipCoord().getX() << " " << shipCoord().getY() << " " << shipCoord().getZ() << " "; // std::cout << "speed: " << radar->targetSpeed().getX() << " " << radar->targetSpeed().getY() << " " << // radar->targetSpeed().getZ() << " "; // std::cout << "angular: " << m_object->angularVelocity().getX() << " " << m_object->angularVelocity().getY() << " "; // std::cout << "thrust: " << thrust << " " << thrustY << " "; // std::cout << "power: " << power << " "; // std::cout << "premtion: " << preemption().getX() << " " << preemption().getY() << " " << preemption().getZ() << " "; // std::cout << "\n"; m_object->run(frameRate); }
void LLCamera::calculateWorldFrustumPlanes() { F32 d; LLVector3 center = mOrigin - mXAxis*mNearPlane; mWorldPlanePos = center; for (int p=0; p<4; p++) { LLVector3 pnorm = LLVector3(mLocalPlanes[p]); LLVector3 norm = rotateToAbsolute(pnorm); norm.normVec(); d = -(center * norm); mWorldPlanes[p] = LLPlane(norm, d); } // horizontal planes, perpindicular to (0,0,1); LLVector3 zaxis(0, 0, 1.0f); F32 yaw = getYaw(); { LLVector3 tnorm = LLVector3(mLocalPlanes[PLANE_LEFT]); tnorm.rotVec(yaw, zaxis); d = -(mOrigin * tnorm); mHorizPlanes[HORIZ_PLANE_LEFT] = LLPlane(tnorm, d); } { LLVector3 tnorm = LLVector3(mLocalPlanes[PLANE_RIGHT]); tnorm.rotVec(yaw, zaxis); d = -(mOrigin * tnorm); mHorizPlanes[HORIZ_PLANE_RIGHT] = LLPlane(tnorm, d); } }
void radioTransceiverTask(const void *arg) { unsigned long tv; static unsigned long radio_last_tv2 = 0; while (true){ #if 1 #if 0 //debug tv=millis(); Serial.println(tv-radio_last_tv2); radio_last_tv2=tv; #endif memset(transceiverBuffer, '\0', sizeof(transceiverBuffer)); sprintf(transceiverBuffer, "@%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%d:%d:%d:%d:%d#", getRoll(), getPitch(), getYaw(), getPidSp(&rollAttitudePidSettings), getPidSp(&pitchAttitudePidSettings), getYawCenterPoint() + getPidSp(&yawAttitudePidSettings), getRollGyro(), getPitchGyro(), getYawGyro(), getThrottlePowerLevel(), getMotorPowerLevelCCW1(), getMotorPowerLevelCW1(), getMotorPowerLevelCCW2(), getMotorPowerLevelCW2()); Udp.beginPacket(broadcastIP, localPort); Udp.write(transceiverBuffer,strlen(transceiverBuffer)); Udp.endPacket(); increasePacketAccCounter(); os_thread_yield(); delay(TRANSMIT_TIMER); #endif } }
bool Player::toggleVanityMode(bool enable, bool force) { if ((mVanity.forced && !force) || (!mVanity.allowed && (force || enable))) { return false; } else if (mVanity.enabled == enable) { return true; } mVanity.enabled = enable; mVanity.forced = force && enable; float offset = mPreviewCam.offset; Ogre::Vector3 rot(0.f, 0.f, 0.f); if (mVanity.enabled) { rot.x = Ogre::Degree(-30.f).valueRadians(); mMainCam.offset = mCamera->getPosition().z; setLowHeight(true); } else { rot.x = getPitch(); offset = mMainCam.offset; setLowHeight(!mFirstPersonView); } rot.z = getYaw(); mCamera->setPosition(0.f, 0.f, offset); rotateCamera(rot, false); return true; }
bool Camera::toggleVanityMode(bool enable) { // Changing the view will stop all playing animations, so if we are playing // anything important, queue the view change for later if (!mPreviewMode) { mVanityToggleQueued = true; return false; } if(!mVanity.allowed && enable) return false; if(mVanity.enabled == enable) return true; mVanity.enabled = enable; processViewChange(); float offset = mPreviewCam.offset; Ogre::Vector3 rot(0.f, 0.f, 0.f); if (mVanity.enabled) { rot.x = Ogre::Degree(-30.f).valueRadians(); mMainCam.offset = mCamera->getPosition().z; } else { rot.x = getPitch(); offset = mMainCam.offset; } rot.z = getYaw(); mCamera->setPosition(0.f, 0.f, offset); rotateCamera(rot, false); return true; }
bool Location::equals(const Location &other) const { return getRegion() == other.getRegion() && getX() == other.getX() && getY() == other.getY() && getZ() == other.getZ() && getYaw() == other.getYaw()&& getPitch() == other.getPitch(); }
void Camera::rotateCamera(const Ogre::Vector3 &rot, bool adjust) { if (adjust) { setYaw(getYaw() + rot.z); setPitch(getPitch() + rot.x); } else { setYaw(rot.z); setPitch(rot.x); } Ogre::Quaternion xr(Ogre::Radian(getPitch() + Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_X); if (!mVanity.enabled && !mPreviewMode) { mCamera->getParentNode()->setOrientation(xr); } else { Ogre::Quaternion zr(Ogre::Radian(getYaw()), Ogre::Vector3::UNIT_Z); mCamera->getParentNode()->setOrientation(zr * xr); } }
void Camera::rotateCamera(float pitch, float yaw, bool adjust) { if (adjust) { pitch += getPitch(); yaw += getYaw(); } setYaw(yaw); setPitch(pitch); }
void serialComms(){ if (Serial.available()) { s = Serial.readStringUntil('\n'); if(s.equals("zero")){ encoderArm.zero(); integral = 0; derivative = 0; setPoint = 0; } else if(s.equals("pollYaw") && mpuEnabled) pollYaw ^= 1; else if(s.equals("zeroYaw") && mpuEnabled){ yawOffset = getYaw(); yintegral = 0; yderivative = 0; } else if(s.equals("rawYaw")) yawOffset = 0; else if(s.startsWith("kp=")){ char c[s.substring(3).length()+1]; s.substring(3).toCharArray(c,sizeof(c)); lKP = atof(c); } else if(s.startsWith("ki=")){ char c[s.substring(3).length()+1]; s.substring(3).toCharArray(c,sizeof(c)); lKI = atof(c); } else if(s.startsWith("kd=")){ char c[s.substring(3).length()+1]; s.substring(3).toCharArray(c,sizeof(c)); lKD = atof(c); } else if(s.startsWith("gripper")){ char c[s.substring(7).length()+1]; s.substring(7).toCharArray(c,sizeof(c)); gripper.write(constrain(atoi(c),0,180)); } else{ int n = s.toInt(); setPoint = n; //yintegral = 0; //theta = n; } Serial.read(); } //if(pollYaw && mpuEnabled) Serial.print(yaw); Serial.print("\t"); Serial.println(youtput); //Serial.print(followerL[5]); //Serial.print("\t"); //Serial.println(followerM[1]); }
inline std::ostream& operator << (std::ostream& io, base::Pose const& pose) { io << "Position " << pose.position.transpose() << " Orientation (RPY)" << getRoll(pose.orientation) << " " << getPitch(pose.orientation) << " " << getYaw(pose.orientation); ; return io; }
/** * update attitude * * @param * void * * @return * void * */ void attitudeUpdate(){ float yrpAttitude[3]; float pryRate[3]; float xyzAcc[3]; float xComponent[3]; float yComponent[3]; float zComponent[3]; float xyzMagnet[3]; #if CHECK_ATTITUDE_UPDATE_LOOP_TIME struct timeval tv_c; static struct timeval tv_l; unsigned long timeDiff=0; gettimeofday(&tv_c,NULL); timeDiff=GET_USEC_TIMEDIFF(tv_c,tv_l); _DEBUG(DEBUG_NORMAL,"attitude update duration=%ld us\n",timeDiff); UPDATE_LAST_TIME(tv_c,tv_l); #endif getYawPitchRollInfo(yrpAttitude, pryRate, xyzAcc, xComponent, yComponent, zComponent,xyzMagnet); setYaw(yrpAttitude[0]); setRoll(yrpAttitude[1]); setPitch(yrpAttitude[2]); setYawGyro(-pryRate[2]); setPitchGyro(pryRate[0]); setRollGyro(-pryRate[1]); setXAcc(xyzAcc[0]); setYAcc(xyzAcc[1]); setZAcc(xyzAcc[2]); setXGravity(zComponent[0]); setYGravity(zComponent[1]); setZGravity(zComponent[2]); setVerticalAcceleration(deadband((getXAcc() * zComponent[0] + getYAcc() * zComponent[1] + getZAcc() * zComponent[2] - 1.f) * 100.f,3.f) ); setXAcceleration(deadband((getXAcc() * xComponent[0] + getYAcc() * xComponent[1] + getZAcc() * xComponent[2]) * 100.f,3.f) ); setYAcceleration(deadband((getXAcc() * yComponent[0] + getYAcc() * yComponent[1] + getZAcc() * yComponent[2]) * 100.f,3.f) ); _DEBUG(DEBUG_ATTITUDE, "(%s-%d) ATT: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n", __func__, __LINE__, getRoll(), getPitch(), getYaw()); _DEBUG(DEBUG_GYRO, "(%s-%d) GYRO: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n", __func__, __LINE__, getRollGyro(), getPitchGyro(), getYawGyro()); _DEBUG(DEBUG_ACC, "(%s-%d) ACC: x=%3.3f y=%3.3f z=%3.3f\n", __func__, __LINE__, getXAcc(), getYAcc(), getZAcc()); }
void AIController::rotateX() { bool decreased = false; if (fabs(shipCoord().getX()) < EPS || fabs(getYaw()) >= fabs(MAX_ROTATE_POWER + shipCoord().getX() / ROTATE_CONST)) { // std::cout << "#RX " << getYaw() << " "; decreased = true; if (fabs(getYaw()) > ZERO_ANGULAR) { if (getYaw() > 0) { minusYaw(); } else { plusYaw(); } } } if (shipCoord().getX() > EPS && getYaw() < MAX_ROTATE_POWER + shipCoord().getX() / ROTATE_CONST && !decreased) { // std::cout << "!RX " << getYaw() << " "; plusYaw(); } if (shipCoord().getX() < -EPS && getYaw() > -MAX_ROTATE_POWER - shipCoord().getX() / ROTATE_CONST && !decreased) { // std::cout << "@RX " << getYaw() << " "; minusYaw(); } }
Robot::Robot(Ogre::String _name, Ogre::SceneManager* _sceneMgr, Ogre::Real _height) : Enemy(_name, _sceneMgr, _height, "robot.mesh") { printf("Robot created\n"); animationState = entity->getAnimationState("Idle"); animationState->setLoop(true); animationState->setEnabled(true); speed = 0.01f; initialForward = Ogre::Vector3::UNIT_X; printf("initial rot %f\n", getYaw().valueDegrees()); }
void Camera::updateCamera(osg::Camera *cam) { if (mTrackingPtr.isEmpty()) return; osg::Vec3d position = getFocalPoint(); osg::Quat orient = osg::Quat(getPitch(), osg::Vec3d(1,0,0)) * osg::Quat(getYaw(), osg::Vec3d(0,0,1)); osg::Vec3d offset = orient * osg::Vec3d(0, -mCameraDistance, 0); position += offset; osg::Vec3d forward = orient * osg::Vec3d(0,1,0); osg::Vec3d up = orient * osg::Vec3d(0,0,1); cam->setViewMatrixAsLookAt(position, position + forward, up); }
/** * get the output of attitude PID controler, this output will become a input for angular velocity PID controler * * @param * void * * @return * value * */ void getAttitudePidOutput() { rollAttitudeOutput = LIMIT_MIN_MAX_VALUE( pidCalculation(&rollAttitudePidSettings, getRoll(),true,true,true), -getGyroLimit(), getGyroLimit()); pitchAttitudeOutput = LIMIT_MIN_MAX_VALUE( pidCalculation(&pitchAttitudePidSettings, getPitch(),true,true,true), -getGyroLimit(), getGyroLimit()); yawAttitudeOutput = LIMIT_MIN_MAX_VALUE( pidCalculation(&yawAttitudePidSettings, yawTransform(getYaw()),true,true,true), -getGyroLimit(), getGyroLimit()); _DEBUG(DEBUG_ATTITUDE_PID_OUTPUT, "(%s-%d) attitude pid output: roll=%.5f, pitch=%.5f, yaw=%.5f\n", __func__, __LINE__, rollAttitudeOutput, pitchAttitudeOutput, yawAttitudeOutput); }
int Rotate::update() { double fdest = dest + startRot; //@TODO Move this into a class member, calculated at enter() if (fdest > 360.0) fdest -= 360.0; //THIS COST 3 HOURS double diff = getYaw() - fdest; if (diff > 180.0) diff = 360.0 - diff; if (diff < -180.0) diff = 360 + diff; drive(0, (-diff * ROTATE_SCALAR) + std::copysign(-diff, ROT_STDSTATE_COR)); if (fabs(diff) <= 3.0) return TIMER_EXPIRED; return NO_UPDATE; }
void Player::togglePreviewMode(bool enable) { if (mPreviewMode == enable) { return; } mPreviewMode = enable; float offset = mCamera->getPosition().z; if (mPreviewMode) { mMainCam.offset = offset; offset = mPreviewCam.offset; setLowHeight(true); } else { mPreviewCam.offset = offset; offset = mMainCam.offset; setLowHeight(!mFirstPersonView); } mCamera->setPosition(0.f, 0.f, offset); rotateCamera(Ogre::Vector3(getPitch(), 0.f, getYaw()), false); }
void HMC5883L::Calibration(int count) { //평균 계산을 위한 변수 float average_offset = 0; int16_t tmp_x = 0, tmp_y = 0, tmp_z = 0; //현재 offset 초기화 Heading_offset = 0; for(int i = 0; i < count; i++) { float tmp_yaw; getHeading(&tmp_x, &tmp_y, &tmp_z); getYaw(tmp_x, tmp_y, &tmp_yaw); if(tmp_yaw < 0) tmp_yaw *= -1; average_offset += tmp_yaw; } //새로운 offset 적용 Heading_offset = average_offset / count; }
bool SpektrumRX::initOkay(void){ // check if all sticks are centered and land selected all is okay if (getThrottle() == 0){ if (getRoll() == 0){ if (getPitch() == 0){ if (getYaw() == 0){ if (getAuto() == 0) return true; else return false; } else return false; } else return false; } else return false; } else return false; }
void RoboState::faceDestination() { ROS_INFO("Calling face destination."); // how much we are off the goal double yawOffset = getYawGoal() - getYaw(); // means that we are off by more than 90 degrees if(yawOffset > 90) { ROS_INFO("We are turning left because we were off by %f)", yawOffset); ROS_INFO("(Should be more than 90 degrees)"); rotateLeft_90(); ROS_INFO("Assume that we turned 90 degrees to the leftproperly."); } // means that we are off by less than -90 degrees else if(yawOffset < -90){ ROS_INFO("We are turning right because yawOffset was %f", yawOffset); ROS_INFO("(Should be less than -90 degrees)"); rotateRight_90(); } // means that we are off by between 90 and -90 degrees else if(yawOffset >= ANGLE_ERR || yawOffset <= -ANGLE_ERR){ ROS_INFO("We are turning less because yawOffset was %f", yawOffset); ROS_INFO("(Should be between 90 and -90 degrees)"); usleep(MOVEMENT_INTERVAL); this->velocityCommand.linear.x = 0; if(yawOffset >= 0){ this->velocityCommand.angular.z = MIN_LEFT; } else this->velocityCommand.angular.z = MIN_RIGHT; velocityPublisher.publish(this->velocityCommand); } else{ setCurrentState(NEUTRAL); } }
/* * This method accepts only initial pose estimates in the global frame, #5148. * * ************** N.B. This method has been rewritten by DB ***************** */ void AMCLocalizer::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { tf::Pose pose_new; tf::poseMsgToTF(msg->pose.pose, pose_new); // Re-initialize the filter pf_vector_t pf_init_pose_mean = pf_vector_zero(); pf_init_pose_mean.v[0] = pose_new.getOrigin().x(); pf_init_pose_mean.v[1] = pose_new.getOrigin().y(); pf_init_pose_mean.v[2] = getYaw(pose_new); pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); // Copy in the covariance, converting from 6-D to 3-D for(int i=0; i<2; i++) for(int j=0; j<2; j++) pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j]; pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5]; delete initial_pose_hyp_; initial_pose_hyp_ = new amcl_hyp_t(); initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; applyInitialPose(); }
void code(){ lastTime = time; time = micros(); dt = (time-lastTime)/1000000; followerL[0] = dt; followerM[0] = dt; followerR[0] = dt; lineFollow(followerL); lineFollow(followerM); lineFollow(followerR); //gyroPID(); if(followerM[1] > 700 && followerL[1] > 700 && followerR[1] <= 700){ lastRead = 2; } if(followerM[1] <= 700 && followerL[1] > 700 && followerR[1] > 700){ lastRead = 1; } if(followerM[1] > 700 && followerL[1] <= 700 && followerR[1] <= 700){ lastRead = 0; } if(followerM[1] <= 700 || followerL[1] <= 700 || followerR[1] <= 700){ //driveL.write(left); driveL.write(90+followerL[5]-followerR[5]+25); //driveR.write(right); driveR.write(90-followerR[5]+followerL[5]-25); } else{ if(lastRead == 2){ driveL.write(70); driveR.write(110); } else if(lastRead == 1){ driveL.write(110); driveR.write(70); } else{ driveL.write(90); driveR.write(90); } } serialComms(); yaw = getYaw() - yawOffset; lastError = error; error = setPoint - encoderArm.getPosition(); if(dt != 0) derivative = (error-lastError)/dt; else derivative = 0; integral += error*dt; output = error*kP + integral*kI + derivative*kD; if(abs(output) > 90) output = 90*(abs(output)/output); if(output < -30) armMotor.write((-30)+90); else if(output > 50) armMotor.write((50)+90); else armMotor.write(output+90); }
void LocalPlayer::applyControl(float dtime) { // Clear stuff swimming_vertical = false; setPitch(control.pitch); setYaw(control.yaw); // Nullify speed and don't run positioning code if the player is attached if(isAttached) { setSpeed(v3f(0,0,0)); return; } v3f move_direction = v3f(0,0,1); move_direction.rotateXZBy(getYaw()); v3f speedH = v3f(0,0,0); // Horizontal (X, Z) v3f speedV = v3f(0,0,0); // Vertical (Y) bool fly_allowed = m_gamedef->checkLocalPrivilege("fly"); bool fast_allowed = m_gamedef->checkLocalPrivilege("fast"); bool free_move = fly_allowed && g_settings->getBool("free_move"); bool fast_move = fast_allowed && g_settings->getBool("fast_move"); // When aux1_descends is enabled the fast key is used to go down, so fast isn't possible bool fast_climb = fast_move && control.aux1 && !g_settings->getBool("aux1_descends"); bool continuous_forward = g_settings->getBool("continuous_forward"); bool always_fly_fast = g_settings->getBool("always_fly_fast"); // Whether superspeed mode is used or not bool superspeed = false; if (always_fly_fast && free_move && fast_move) superspeed = true; // Old descend control if(g_settings->getBool("aux1_descends")) { // If free movement and fast movement, always move fast if(free_move && fast_move) superspeed = true; // Auxiliary button 1 (E) if(control.aux1) { if(free_move) { // In free movement mode, aux1 descends if(fast_move) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_walk; } else if(in_liquid || in_liquid_stable) { speedV.Y = -movement_speed_walk; swimming_vertical = true; } else if(is_climbing) { speedV.Y = -movement_speed_climb; } else { // If not free movement but fast is allowed, aux1 is // "Turbo button" if(fast_move) superspeed = true; } } } // New minecraft-like descend control else { // Auxiliary button 1 (E) if(control.aux1) { if(!is_climbing) { // aux1 is "Turbo button" if(fast_move) superspeed = true; } } if(control.sneak) { if(free_move) { // In free movement mode, sneak descends if (fast_move && (control.aux1 || always_fly_fast)) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_walk; } else if(in_liquid || in_liquid_stable) { if(fast_climb) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_walk; swimming_vertical = true; } else if(is_climbing) { if(fast_climb) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_climb; } } } if (continuous_forward) speedH += move_direction; if (control.up) { if (continuous_forward) { if (fast_move) superspeed = true; } else { speedH += move_direction; } } if(control.down) { speedH -= move_direction; } if(control.left) { speedH += move_direction.crossProduct(v3f(0,1,0)); } if(control.right) { speedH += move_direction.crossProduct(v3f(0,-1,0)); } if(control.jump) { if (free_move) { if (g_settings->getBool("aux1_descends") || always_fly_fast) { if (fast_move) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_walk; } else { if(fast_move && control.aux1) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_walk; } } else if(m_can_jump) { /* NOTE: The d value in move() affects jump height by raising the height at which the jump speed is kept at its starting value */ v3f speedJ = getSpeed(); if(speedJ.Y >= -0.5 * BS) { speedJ.Y = movement_speed_jump * physics_override_jump; setSpeed(speedJ); MtEvent *e = new SimpleTriggerEvent("PlayerJump"); m_gamedef->event()->put(e); } } else if(in_liquid) { if(fast_climb) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_walk; swimming_vertical = true; } else if(is_climbing) { if(fast_climb) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_climb; } } // The speed of the player (Y is ignored) if(superspeed || (is_climbing && fast_climb) || ((in_liquid || in_liquid_stable) && fast_climb)) speedH = speedH.normalize() * movement_speed_fast; else if(control.sneak && !free_move && !in_liquid && !in_liquid_stable) speedH = speedH.normalize() * movement_speed_crouch; else speedH = speedH.normalize() * movement_speed_walk; // Acceleration increase f32 incH = 0; // Horizontal (X, Z) f32 incV = 0; // Vertical (Y) if((!touching_ground && !free_move && !is_climbing && !in_liquid) || (!free_move && m_can_jump && control.jump)) { // Jumping and falling if(superspeed || (fast_move && control.aux1)) incH = movement_acceleration_fast * BS * dtime; else incH = movement_acceleration_air * BS * dtime; incV = 0; // No vertical acceleration in air } else if (superspeed || (is_climbing && fast_climb) || ((in_liquid || in_liquid_stable) && fast_climb)) incH = incV = movement_acceleration_fast * BS * dtime; else incH = incV = movement_acceleration_default * BS * dtime; // Accelerate to target speed with maximum increment accelerateHorizontal(speedH * physics_override_speed, incH * physics_override_speed); accelerateVertical(speedV * physics_override_speed, incV * physics_override_speed); }
void LocalPlayer::applyControl(float dtime, ClientEnvironment *env) { // Clear stuff swimming_vertical = false; setPitch(control.pitch); setYaw(control.yaw); // Nullify speed and don't run positioning code if the player is attached if(isAttached) { setSpeed(v3f(0,0,0)); return; } v3f move_direction = v3f(0,0,1); move_direction.rotateXZBy(getYaw()); v3f speedH = v3f(0,0,0); // Horizontal (X, Z) v3f speedV = v3f(0,0,0); // Vertical (Y) bool fly_allowed = m_gamedef->checkLocalPrivilege("fly"); bool fast_allowed = m_gamedef->checkLocalPrivilege("fast"); free_move = fly_allowed && g_settings->getBool("free_move"); bool fast_move = fast_allowed && g_settings->getBool("fast_move"); // When aux1_descends is enabled the fast key is used to go down, so fast isn't possible bool fast_climb = fast_move && control.aux1 && !g_settings->getBool("aux1_descends"); bool continuous_forward = g_settings->getBool("continuous_forward"); bool fast_pressed = false; bool always_fly_fast = g_settings->getBool("always_fly_fast"); // Whether superspeed mode is used or not superspeed = false; if (always_fly_fast && free_move && fast_move) superspeed = true; // Old descend control if(g_settings->getBool("aux1_descends")) { // If free movement and fast movement, always move fast if(free_move && fast_move) superspeed = true; // Auxiliary button 1 (E) if(control.aux1) { if(free_move) { // In free movement mode, aux1 descends if(fast_move) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_walk; } else if(in_liquid || in_liquid_stable) { speedV.Y = -movement_speed_walk; swimming_vertical = true; } else if(is_climbing) { speedV.Y = -movement_speed_climb; } else { // If not free movement but fast is allowed, aux1 is // "Turbo button" if(fast_allowed) superspeed = true; } } } // New minecraft-like descend control else { // Auxiliary button 1 (E) if(control.aux1) { if(!is_climbing) { // aux1 is "Turbo button" if(fast_allowed) superspeed = true; } if(fast_allowed) fast_pressed = true; } if(control.sneak) { if(free_move) { // In free movement mode, sneak descends if (fast_move && (control.aux1 || always_fly_fast)) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_walk; } else if(in_liquid || in_liquid_stable) { if(fast_climb) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_walk; swimming_vertical = true; } else if(is_climbing) { if(fast_climb) speedV.Y = -movement_speed_fast; else speedV.Y = -movement_speed_climb; } } } if (continuous_forward) speedH += move_direction; if (control.up) { if (continuous_forward) { if (fast_move) superspeed = true; } else { speedH += move_direction; } } if(control.down) { speedH -= move_direction; } if(control.left) { speedH += move_direction.crossProduct(v3f(0,1,0)); } if(control.right) { speedH += move_direction.crossProduct(v3f(0,-1,0)); } if(control.jump) { if (free_move) { if (g_settings->getBool("aux1_descends") || always_fly_fast) { if (fast_move) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_walk; } else { if(fast_move && control.aux1) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_walk; } } else if(m_can_jump) { /* NOTE: The d value in move() affects jump height by raising the height at which the jump speed is kept at its starting value */ v3f speedJ = getSpeed(); if(speedJ.Y >= -0.5 * BS) { speedJ.Y = movement_speed_jump * physics_override_jump; setSpeed(speedJ); MtEvent *e = new SimpleTriggerEvent("PlayerJump"); m_gamedef->event()->put(e); } } else if(in_liquid) { if(fast_climb) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_walk; swimming_vertical = true; } else if(is_climbing) { if(fast_climb) speedV.Y = movement_speed_fast; else speedV.Y = movement_speed_climb; } } // The speed of the player (Y is ignored) if(superspeed || (is_climbing && fast_climb) || ((in_liquid || in_liquid_stable) && fast_climb) || fast_pressed) speedH = speedH.normalize() * movement_speed_fast; else if(control.sneak && !free_move && !in_liquid && !in_liquid_stable) speedH = speedH.normalize() * movement_speed_crouch; else speedH = speedH.normalize() * movement_speed_walk; // Acceleration increase f32 incH = 0; // Horizontal (X, Z) f32 incV = 0; // Vertical (Y) if((!touching_ground && !free_move && !is_climbing && !in_liquid) || (!free_move && m_can_jump && control.jump)) { // Jumping and falling if(superspeed || (fast_move && control.aux1)) incH = movement_acceleration_fast * BS * dtime; else incH = movement_acceleration_air * BS * dtime; incV = 0; // No vertical acceleration in air // better air control when falling fast float speed = m_speed.getLength(); if (!superspeed && speed > movement_speed_fast && (control.down || control.up || control.left || control.right)) { v3f rotate = move_direction * (speed / (movement_fall_aerodynamics * BS)); if(control.up) rotate = rotate.crossProduct(v3f(0,1,0)); if(control.down) rotate = rotate.crossProduct(v3f(0,-1,0)); if(control.left) rotate *=-1; m_speed.rotateYZBy(rotate.X); m_speed.rotateXZBy(rotate.Y); m_speed.rotateXYBy(rotate.Z); m_speed = m_speed.normalize() * speed * (1-speed*0.00001); // 0.998 if (m_speed.Y) return; } } else if (superspeed || (is_climbing && fast_climb) || ((in_liquid || in_liquid_stable) && fast_climb)) incH = incV = movement_acceleration_fast * BS * dtime; else incH = incV = movement_acceleration_default * BS * dtime; // Accelerate to target speed with maximum increment INodeDefManager *nodemgr = m_gamedef->ndef(); Map *map = &env->getMap(); v3s16 p = floatToInt(getPosition() - v3f(0,BS/2,0), BS); float slippery = 0; try { slippery = itemgroup_get(nodemgr->get(map->getNode(p)).groups, "slippery"); } catch (...) {} accelerateHorizontal(speedH * physics_override_speed, incH * physics_override_speed, slippery); accelerateVertical(speedV * physics_override_speed, incV * physics_override_speed); }
void Robot::moveTo(Location location) { Location target = getStageLocation(location); player_color cyan; cyan.red = 0; cyan.green = 255; cyan.blue = 255; cyan.alpha = 255; double biggestSize = getWidth() > getHeight() ? getWidthMeters() : getHeightMeters(); pp->SetMotorEnable(true); pp->SetSpeed(0, 0); double d; while((d = distanceTo(target)) > biggestSize/2) { pc->Read(); gp->Clear(); Position currentPosition(pp->GetXPos(), pp->GetYPos(), pp->GetYaw()); setPosition(currentPosition); drawPoint(target, cyan); player_point_2d points[2]; points[0].px = getX(); points[0].py = getY(); points[1].px = target.getX(); points[1].py = target.getY(); gp->Color(255, 0, 0, 255); gp->DrawPolyline(points, 2); double targetYaw = angleTo(target); double yawDiff = targetYaw - getYaw(); while(yawDiff < dtor(180)) yawDiff += dtor(360); while(yawDiff > dtor(180)) yawDiff -= dtor(360); double speed; if(abs(yawDiff) < 0.1) speed = maxSpeed; else if(abs(yawDiff) > dtor(45)) speed = 0; else speed = maxSpeed * (1 - (abs(yawDiff) / dtor(45))); if(speed > maxSpeed) speed = maxSpeed; double turnSpeed = yawDiff; if(abs(turnSpeed) > maxTurnSpeed) turnSpeed = (turnSpeed/abs(turnSpeed))*maxTurnSpeed; pp->SetSpeed(speed, turnSpeed); } pp->SetSpeed(0, 0); pp->SetMotorEnable(false); }
void LocalPlayer::applyControl(float dtime) { // Clear stuff swimming_up = false; // Random constants f32 walk_acceleration = 4.0 * BS; f32 walkspeed_max = 4.0 * BS; setPitch(control.pitch); setYaw(control.yaw); v3f move_direction = v3f(0,0,1); move_direction.rotateXZBy(getYaw()); v3f speed = v3f(0,0,0); bool free_move = g_settings->getBool("free_move"); bool fast_move = g_settings->getBool("fast_move"); bool continuous_forward = g_settings->getBool("continuous_forward"); if(free_move || is_climbing) { v3f speed = getSpeed(); speed.Y = 0; setSpeed(speed); } // Whether superspeed mode is used or not bool superspeed = false; // If free movement and fast movement, always move fast if(free_move && fast_move) superspeed = true; // Auxiliary button 1 (E) if(control.aux1) { if(free_move) { // In free movement mode, aux1 descends v3f speed = getSpeed(); if(fast_move) speed.Y = -20*BS; else speed.Y = -walkspeed_max; setSpeed(speed); } else if(is_climbing) { v3f speed = getSpeed(); speed.Y = -3*BS; setSpeed(speed); } else { // If not free movement but fast is allowed, aux1 is // "Turbo button" if(fast_move) superspeed = true; } } if(continuous_forward) speed += move_direction; if(control.up) { if(continuous_forward) superspeed = true; else speed += move_direction; } if(control.down) { speed -= move_direction; } if(control.left) { speed += move_direction.crossProduct(v3f(0,1,0)); } if(control.right) { speed += move_direction.crossProduct(v3f(0,-1,0)); } if(control.jump) { if(free_move) { v3f speed = getSpeed(); if(fast_move) speed.Y = 20*BS; else speed.Y = walkspeed_max; setSpeed(speed); } else if(touching_ground) { v3f speed = getSpeed(); /* NOTE: The d value in move() affects jump height by raising the height at which the jump speed is kept at its starting value */ speed.Y = 6.5*BS; setSpeed(speed); } // Use the oscillating value for getting out of water // (so that the player doesn't fly on the surface) else if(in_water) { v3f speed = getSpeed(); speed.Y = 1.5*BS; setSpeed(speed); swimming_up = true; } else if(is_climbing) { v3f speed = getSpeed(); speed.Y = 3*BS; setSpeed(speed); } } // The speed of the player (Y is ignored) if(superspeed) speed = speed.normalize() * walkspeed_max * 5.0; else if(control.sneak) speed = speed.normalize() * walkspeed_max / 3.0; else speed = speed.normalize() * walkspeed_max; f32 inc = walk_acceleration * BS * dtime; // Faster acceleration if fast and free movement if(free_move && fast_move) inc = walk_acceleration * BS * dtime * 10; // Accelerate to target speed with maximum increment accelerate(speed, inc); }