Exemplo n.º 1
0
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;
    }
}
Exemplo n.º 2
0
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));


}
Exemplo n.º 3
0
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);
    }
}
Exemplo n.º 4
0
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();
}
Exemplo n.º 5
0
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());
}
Exemplo n.º 6
0
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(&currentState.key.gyro.vel.x, &currentState.key.gyro.vel.y, &currentState.key.gyro.vel.z);
	Get_Accel_Angles(&currentState.key.accel.pos.x, &currentState.key.accel.pos.y);
	//Get_Mag_Value_Normalized(&currentState.key.magn.pos.x, &currentState.key.magn.pos.y, &currentState.key.magn.pos.z);

	Get_Accel_Gravity_Power(&currentState.key.accel.vel.x, &currentState.key.accel.vel.y, &currentState.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, &currentState.key.Kalman.acc.x, &currentState.key.Kalman.acc.y, &currentState.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, &currentState.key.Kalman.vel.x, &currentState.key.Kalman.vel.y, &currentState.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 (&currentState.key.Kalman.pos.z, currentState.key.Kalman.pos.x, currentState.key.Kalman.pos.y);
	// Get_Mag_Heading(&currentState.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°
}
Exemplo n.º 7
0
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();
    } 
}
Exemplo n.º 8
0
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);
}
Exemplo n.º 9
0
	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;
	}
Exemplo n.º 10
0
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());
}
Exemplo n.º 11
0
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);
    }
}
Exemplo n.º 12
0
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();
}
Exemplo n.º 13
0
	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);

}
Exemplo n.º 15
0
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());
}
Exemplo n.º 16
0
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());
}
Exemplo n.º 17
0
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());
}
Exemplo n.º 18
0
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();
}
Exemplo n.º 19
0
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;
}
Exemplo n.º 20
0
/**
 * 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);
    }
}
Exemplo n.º 21
0
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);
}
Exemplo n.º 22
0
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);
}
Exemplo n.º 23
0
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);
}
Exemplo n.º 24
0
/*
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;
}
Exemplo n.º 25
0
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);}
}
Exemplo n.º 27
0
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);
	}
}
Exemplo n.º 28
0
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);

}
Exemplo n.º 30
0
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;
}