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