void TouchEvent::addRelatedEvent(TouchEventPtr pEvent)
{
m_RelatedEvents.push_back(pEvent);
if (getSource() == Event::TOUCH && m_RelatedEvents.size() == 1) {
TouchEventPtr pHandEvent = m_RelatedEvents.begin()->lock();
m_HandOrientation = getAngle(pHandEvent->getPos()-getPos());
m_bHasHandOrientation = true;
}
}
double cSatPointCover::getSatPointAngle()
{
cVector3d r=getSatPosECI();
double h=sqrt(r.getPoint3D().x*r.getPoint3D().x
+r.getPoint3D().y*r.getPoint3D().y
+r.getPoint3D().z*r.getPoint3D().z);
return asin(cos(deg2rad(getAngle()))*(RE/h));
}
void LaserSystem::updateRotations(es::World& world)
{
// Update the rotation components based on the direction of the lasers
for (auto ent: world.query<Laser, Rotation>())
{
auto laser = ent.get<Laser>();
auto rotation = ent.get<Rotation>();
rotation->angle = laser->getAngle(laser->direction);
}
}
void Imu::zeroAccel()
{
Imu::Angle angle = getAngle(false);
_accelZeroPitch = angle.pitch;
_accelZeroRoll = angle.roll;
//DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
//_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
//_configFileWrapper.setAccelZeroRoll(_accelZeroRoll);
//_configFileWrapper.saveSettings();
}
void MgEllipse::setRadius(float rx, float ry)
{
rx = fabsf(rx);
ry = fabsf(ry);
if (ry < _MGZERO)
ry = rx;
Box2d rect(getCenter(), rx * 2, ry * 2);
setRectWithAngle(rect.leftTop(), rect.rightBottom(), getAngle(), getCenter());
}
void Callback_20ms() {
desiredState.key.abs.pos.z = altitudeValue / 1000.0;
/* Setting motor speed based on altitude */
lcd_buffer_print(LCD_LINE4, "In: %1.3f", sonarDistance);
outValue = PID_Compute(sonarDistance, desiredState.key.abs.pos.z, &z_axis_PID);
desiredState.key.avg_motor_us = map(outValue * map(analogRead, 0, 4096, 0, 1), 0, 0.5, MOTOR_MIN_US, MOTOR_MAX_US);
lcd_buffer_print(LCD_LINE5, "Mot: %4.0f", desiredState.key.avg_motor_us);
//lcd_buffer_print(LCD_LINE7, "Diff: %3.0f", desiredState.key.motor_diff_us);
/* Obtain accelerometer an gyro values */
Get_Gyro_Rates(¤tState.key.gyro.vel.x, ¤tState.key.gyro.vel.y, ¤tState.key.gyro.vel.z);
Get_Accel_Angles(¤tState.key.accel.pos.x, ¤tState.key.accel.pos.y);
//Get_Mag_Value_Normalized(¤tState.key.magn.pos.x, ¤tState.key.magn.pos.y, ¤tState.key.magn.pos.z);
Get_Accel_Gravity_Power(¤tState.key.accel.vel.x, ¤tState.key.accel.vel.y, ¤tState.key.accel.vel.z);
get_Angle_AHRS(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, ¤tState.key.Kalman.acc.x, ¤tState.key.Kalman.acc.y, ¤tState.key.Kalman.acc.z);
// get_Angle_AHRS_Mahony(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, ¤tState.key.Kalman.vel.x, ¤tState.key.Kalman.vel.y, ¤tState.key.Kalman.vel.z);
/* Calcolo Roll e Pitch con il filtro di Kalman */
currentState.key.Kalman.pos.x = Compute_AVG(getAngle(currentState.key.accel.pos.x, currentState.key.gyro.vel.x, dt, rollKalman), &rollAVG);
currentState.key.Kalman.pos.y = Compute_AVG(getAngle(currentState.key.accel.pos.y, currentState.key.gyro.vel.y, dt, pitchKalman), &pitchAVG);
//currentState.key.Kalman.pos.z = currentState.key.Kalman.acc.z;
// Angolo di Yaw mediante giroscopio
// currentState.key.Kalman.pos.z = currentState.key.gyro.vel.z;
// Get_Mag_Heading_Compensated (¤tState.key.Kalman.pos.z, currentState.key.Kalman.pos.x, currentState.key.Kalman.pos.y);
// Get_Mag_Heading(¤tState.key.Kalman.acc.z);
desiredState.key.x_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.x, 0, &Roll_PID), -30, 30, 60, 120);
desiredState.key.y_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.y, 0, &Pitch_PID), -30, 30, 60, 120);
currentState.key.Kalman.pos.z += currentState.key.gyro.vel.z*dt;
Motor_Write_us(MOTOR_UPPER, desiredState.key.avg_motor_us + desiredState.key.motor_diff_us);
Motor_Write_us(MOTOR_BOTTOM, desiredState.key.avg_motor_us - desiredState.key.motor_diff_us);
Servo_Write_deg(SERVO_ROLL, desiredState.key.x_servo_deg);
Servo_Write_deg(SERVO_PITCH, desiredState.key.y_servo_deg); // pitch servo is trimmed 18°
}
void AngleRadiusWidget::setMinMaxAngle(double minAngle, double maxAngle) {
if (minAngle_ != minAngle || maxAngle_ != maxAngle) {
minAngle_ = minAngle;
maxAngle_ = maxAngle;
// Update radius to be within bounds
setAngle(glm::clamp(getAngle(), getMinAngle(), getMaxAngle()));
update();
emit angleMinMaxChanged();
}
}
void HelloWorld::ccTouchMoved(cocos2d::CCTouch *pTouch, cocos2d::CCEvent *pEvent)
{
CCPoint tar = pTouch->getLocation();
CCPoint new_tar = tar;//ccpSub(tar, m_BeginPoint);
float angle = getAngle(&m_BeginPoint, &new_tar);
CCLOG("ccTouchMoved :%f %f",tar.x,tar.y);
}
AIEXAMPLE_API void Update()
{
tank.movingAngle = tank.baseAngle;
tank.fireAngle = tank.baseAngle;
int angle = getAngle(tank.x, tank.y, tank.baseAngle);
tank.movingAngle = angle;
tank.fireAngle = tank.movingAngle;
}
RS_Vector RS_Ellipse::getNearestOrthTan(const RS_Vector& coord,
const RS_Line& normal,
bool onEntity )
{
if ( !coord.valid ) {
return RS_Vector(false);
}
RS_Vector direction=normal.getEndpoint() - normal.getStartpoint();
if (RS_Vector::dotP(direction,direction)< RS_TOLERANCE*RS_TOLERANCE) {
//undefined direction
return RS_Vector(false);
}
//scale to ellipse angle
direction.rotate(-getAngle());
double angle=direction.scale(RS_Vector(1.,getRatio())).angle();
direction.set(getMajorRadius()*cos(angle),getMinorRadius()*sin(angle));//relative to center
QList<RS_Vector> sol;
for(int i=0; i<2; i++) {
if(!onEntity ||
RS_Math::isAngleBetween(angle,getAngle1(),getAngle2(),isReversed())) {
if(i) {
sol.append(- direction);
} else {
sol.append(direction);
}
}
angle=RS_Math::correctAngle(angle+M_PI);
}
RS_Vector vp;
switch(sol.count()) {
case 0:
return RS_Vector(false);
case 2:
if( RS_Vector::dotP(sol[1]-getCenter(),coord-getCenter())>0.) {
vp=sol[1];
break;
}
default:
vp=sol[0];
}
return getCenter() + vp.rotate(getAngle());
}
void BaseCar::enterLineCallBack()
{
if (getPreviousStateName() == -1) {
return;
}
auto winSize = Director::getInstance()->getWinSize();
auto pos = getPosition();
auto centerX = winSize.width / 2;
auto centerY = winSize.height / 2;
double angle;
double deltaX;
if (pos.x > centerX) {
angle = getAngle(POS_R , pos);
if (pos.y > centerY) {
angle = angle - PI / 2;
setPositionY(POS_L.y + _curRadius);
setRotation3D(Vec3(0 , 0 , 0));
} else {
angle = PI * 3 / 2 - angle;
setPositionY(POS_L.y - _curRadius);
setRotation3D(Vec3(0 , 0 , - 180));
}
deltaX = R_OUTER * angle;
setPositionX(POS_R.x - deltaX);
} else {
angle = getAngle(POS_L , pos);
if (pos.y > centerY) {
angle = PI / 2 - angle;
setPositionY(POS_L.y + _curRadius);
setRotation3D(Vec3(0 , 0 , 0));
} else {
angle = angle - PI * 3 / 2;
setPositionY(POS_L.y - _curRadius);
setRotation3D(Vec3(0 , 0 , - 180));
}
deltaX = R_OUTER * angle;
setPositionX(POS_L.x + deltaX);
}
}
void Cannon::draw(sf::RenderTarget* window) {
//std::cout<< "Drawin Cannon: " << getX() << ", " << getY() << std::endl;
sprite->setPosition(getX(), getY());
sprite->setRotation(getAngle()*180.0f/3.14159f);
window->draw(*sprite);
//pushMatrix();
//translate(getX(), getY());
//rotate(getAngle());
//image(img, -size/2.0, -size/2.0);
//popMatrix();
}
void SfmlBoxInstanceInfo::render()
{
if (Settings::renderDebugBoxes)
{
rectangle.setPosition(getPosition().x, getPosition().y);
rectangle.setRotation(toDegrees(getAngle()));
rectangle.setScale(getScale().x, getScale().y);
rectangle.setOrigin(getPivot().x*getSize().x, getPivot().y*getSize().y);
renderWindow->draw(rectangle);
}
}
float GoToSelectedBall::getAngleDiff(){
float robot_odom_x, robot_odom_y;
getRobotPositionInOdom(robot_odom_x, robot_odom_y);
float goalAngle = getAngle(robot_odom_x, robot_odom_y, getCurrentPose().x, getCurrentPose().y);
float robotAngle = getRobotAngleInOdom();
return fabs(goalAngle-robotAngle);
}
CFontPtr
CFont::
rotated(double dangle) const
{
double angle = getAngle() + dangle;
while (angle < 0 ) angle += 360;
while (angle >= 360) angle -= 360;
return dup(getFamily(), getStyle(), getSize(), angle, getCharAngle(), getXRes(), getYRes());
}
math_vector math_vector::rotate(double degrees){
// printf("%g degrees + %g degrees\n",getAngle(),degrees);
if (!degrees) return math_vector(x,y);
if (degrees==270) return rightAngle();
if (degrees==180) return (*this)*-1;
if (degrees==90) return rightAngle()*-1;
double angle=getAngle()+degrees;
while (angle<0) angle+=360;
while (angle>360) angle-=360;
return createFromPolar(angle,getMagnitude());
}
void XObjectBasic::pushChild(XObjectBasic * child)
{
if(child == NULL ||
child == this || //不能为自身
getIsChild(child)) return;
m_childList.push_back(child);
child->m_parent = this;
m_childRelativePos.push_back((child->getPosition() - getPosition()) / getScale());
m_childRelativeSize.push_back(child->getScale() / getScale());
m_childRelativeAngle.push_back(child->getAngle() - getAngle());
}
void MgEllipse::_update()
{
mgcurv::ellipseToBezier(_bzpts, getCenter(), getWidth() / 2, getHeight() / 2);
Matrix2d mat(Matrix2d::rotation(getAngle(), getCenter()));
for (int i = 0; i < 13; i++)
_bzpts[i] *= mat;
mgnear::beziersBox(_extent, 13, _bzpts, true);
__super::_update();
}
void Perspective::print()
{
cout << "- Camera - Perspective" << endl;
cout << "| ID: " << getID() << endl;
cout << "| ids: " << getIds() << endl;
cout << "| pos: {" << getPos()[0] << ", "<< getPos()[1]<< ", " << getPos()[2] << "}" << endl;
cout << "| near: " << getZNear() << endl;
cout << "| far: " << getZFar() << endl;
cout << "| angle: " << getAngle() << endl;
cout << "| target: {" << getTarget()[0] << ", "<< getTarget()[1] << ", " << getTarget()[2] << "}" << endl;
}
/**
* Update the 2D grid of peak angles
*/
void Gradient::updatePeakGrid()
{
clearPeakGrid();
// Write angle to grid for each peak in list
for(int i=0; isPeak(i); ++i){
peaks[getAnglesYCoord(i) + Gradient::rows/2]
[getAnglesXCoord(i) + Gradient::cols/2] =
getAngle(i);
}
}
void LightningStrikeSpell::Update() {
float fBeta = 0.f;
float falpha = 0.f;
Entity * caster = entities[m_caster];
ObjVertHandle idx = GetGroupOriginByName(caster->obj, "chest");
if(idx != ObjVertHandle()) {
m_caster_pos = caster->obj->vertexWorldPositions[idx.handleData()].v;
} else {
m_caster_pos = caster->pos;
}
if(m_caster == EntityHandle_Player) {
falpha = -player.angle.getPitch();
fBeta = player.angle.getYaw();
} else {
fBeta = caster->angle.getYaw();
if(ValidIONum(caster->targetinfo) && caster->targetinfo != m_caster) {
const Vec3f & p1 = m_caster_pos;
Vec3f p2 = GetChestPos(caster->targetinfo);
falpha = MAKEANGLE(glm::degrees(getAngle(p1.y, p1.z, p2.y, p2.z + glm::distance(Vec2f(p2.x, p2.z), Vec2f(p1.x, p1.z)))));
} else if(ValidIONum(m_target)) {
const Vec3f & p1 = m_caster_pos;
Vec3f p2 = GetChestPos(m_target);
falpha = MAKEANGLE(glm::degrees(getAngle(p1.y, p1.z, p2.y, p2.z + glm::distance(Vec2f(p2.x, p2.z), Vec2f(p1.x, p1.z)))));
}
}
m_lightning.m_pos = m_caster_pos;
m_lightning.m_beta = fBeta;
m_lightning.m_alpha = falpha;
m_lightning.m_caster = m_caster;
m_lightning.m_level = m_level;
m_lightning.Update(g_gameTime.lastFrameDuration());
m_lightning.Render();
ARX_SOUND_RefreshPosition(m_snd_loop, entities[m_caster]->pos);
}
void LightningStrikeSpell::Update(float timeDelta) {
float fBeta = 0.f;
float falpha = 0.f;
Entity * caster = entities[m_caster];
long idx = GetGroupOriginByName(caster->obj, "chest");
if(idx >= 0) {
m_caster_pos = caster->obj->vertexlist3[idx].v;
} else {
m_caster_pos = caster->pos;
}
if(m_caster == PlayerEntityHandle) {
falpha = -player.angle.getYaw();
fBeta = player.angle.getPitch();
} else {
fBeta = caster->angle.getPitch();
if(ValidIONum(caster->targetinfo) && caster->targetinfo != m_caster) {
const Vec3f & p1 = m_caster_pos;
Vec3f p2 = GetChestPos(caster->targetinfo);
falpha = MAKEANGLE(glm::degrees(getAngle(p1.y, p1.z, p2.y, p2.z + glm::distance(Vec2f(p2.x, p2.z), Vec2f(p1.x, p1.z))))); //alpha entre orgn et dest;
} else if(ValidIONum(m_target)) {
const Vec3f & p1 = m_caster_pos;
Vec3f p2 = GetChestPos(m_target);
falpha = MAKEANGLE(glm::degrees(getAngle(p1.y, p1.z, p2.y, p2.z + glm::distance(Vec2f(p2.x, p2.z), Vec2f(p1.x, p1.z))))); //alpha entre orgn et dest;
}
}
m_lightning.m_pos = m_caster_pos;
m_lightning.m_beta = fBeta;
m_lightning.m_alpha = falpha;
m_lightning.m_caster = m_caster;
m_lightning.m_level = m_level;
m_lightning.Update(timeDelta);
m_lightning.Render();
ARX_SOUND_RefreshPosition(m_snd_loop, entities[m_caster]->pos);
}
void SFRotation::getValue(jobject field, int bConstField) {
assert(field);
JNIEnv *jniEnv = getJniEnv();
jclass classid = bConstField ? getConstFieldID() : getFieldID();
jmethodID setValueMethod = bConstField ? getConstSetValueMethodID() : getSetValueMethodID();
assert(classid && setValueMethod);
jfloat x = getX();
jfloat y = getY();
jfloat z = getZ();
jfloat angle = getAngle();
jniEnv->CallVoidMethod(field, setValueMethod, x, y, z, angle);
}
/*
compute the inner product of the vectors, (n+, n-)
*/
double innerProduct(tVertex site, tVertex pole, tVertex v) {
double dist1, dist2;
double angle = 0.0;
double result = 0.0;
dist1 = pointDist(site, pole);
dist2 = pointDist(site, v);
angle = getAngle(pole, v);
result = dist1 * dist2* cos(angle);
return result;
}
void ArrowTower::rotateAndShoot(float dt) {
checkNearestEnemy();
if (nearestEnemy != NULL) {
auto rotateVector = nearestEnemy->getPosition() - this->getPosition();
float rotateradians = rotateVector.getAngle();
float rotateDegrees = CC_RADIANS_TO_DEGREES(-1 * rotateradians);
float speed = 0.5 / M_PI;
float rotateDuration = fabs(rotateradians * speed);
rotateArrow->runAction(Sequence::create(RotateTo::create(rotateDuration, rotateDegrees),
CallFunc::create(CC_CALLBACK_0(ArrowTower::shoot, this)),NULL));
}
}
void TWRotateMultiSprite::draw() const {
Uint32 x = static_cast<Uint32>(X());
Uint32 y = static_cast<Uint32>(Y());
double angle = getAngle();
if (!flag)
{ frames[currentFrame]->draw(x, y, angle);}
else
{
framesMirror[currentFrame]-> draw(x,y, angle);}
}
void ComponentAttachParticleSystem::update(float) {
if (!active) return;
ParticleSystem *system = *handle;
if (system) {
const vec2 x = lastReportedOrientation.getAxisX().xy();
const float rotation = getAngle(x, vec2(1,0));
system->setRotation(rotation);
system->setPosition(lastReportedPosition);
}
}
void Pterodactyl::setOutputRange() {
float angle = getAngle();
#if PID_EQUATION_METHOD
pid->m_maximumITerm = 0.5;
double p, i, d;
if (target>40) {
// float volts = DriverStation::GetInstance()->GetBatteryVoltage(); TODO Can I make it adaptive?
float target = this->target;
if (target > 90) {
target = 90;
}
p = 1615.3*pow(target, -1.5);
i = 2.25*pow(2.71, -.065*target);
d = 97.34*pow(target, -0.85);
d /= 2.0;
if (fabs(initialError) < 30) { // Extra corrections
float divider = fabs(initialError);
if (divider < 10.0) {
divider = 10.0;
}
if (initialError < 0) {
p *= 25.0 / divider;
i *= 125.0 / divider;
} else {
p *= 25.0 / divider;
i *= 200.0 / divider;
}
}
} else if (target > 20) {
pid->m_maximumITerm = 0.150;
p = 2.5/1.5;
i = 0.25;
d = 0.25;
} else {
p = .75;
i = .0105;
d = .05;
}
pid->SetPID(p, i, d);
#else
pid->m_maximumITerm = 0.150;
#endif
if (initialError> 0) {
if (angle < 25) {
pid->SetOutputRange(-.25, 0.75);
} else {
pid->SetOutputRange(-.75, 0.75);
}
} else {
pid->SetOutputRange(-.35, 1.0);
}
}
void Explore::poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
// ROS_INFO("I heard pose (x, y) = (%f, %f)", msg->pose.pose.position.x, msg->pose.pose.position.y);
// aktualizacja pozycji robota
currPose.x = msg->pose.pose.position.x;
currPose.y = msg->pose.pose.position.y;
currPose.theta = getAngle(msg->pose.pose.orientation);
// ROS_INFO("currPose.theta= %f", currPose.theta);
}
float DkRotatingRect::getAngleDeg() const {
float sAngle = (float)(getAngle()*DK_RAD2DEG);
while (sAngle > 90)
sAngle -= 180;
while (sAngle < -90)
sAngle += 180;
sAngle = qRound(sAngle*100)/100.0f; // round to 2 digits
return sAngle;
}
