bool RelativePositionDetection::execute() {
// TODO add angle of objects to panAngle and tiltAngle
// TODO use object size in image for distance approximation
mPanAngle = DEGREE_TO_RADIAN * getBodyStatus().getPan();
mTiltAngle = DEGREE_TO_RADIAN * getBodyStatus().getTilt();
const RobotConfiguration& botConf = getRobotConfiguration();
const CameraSettings& camSet = getCameraSettings();
mRadiansPerPixelH = DEGREE_TO_RADIAN * botConf.cameraOpeningHorizontal
/ (double) (camSet.width);
mRadiansPerPixelV = DEGREE_TO_RADIAN * botConf.cameraOpeningVertical
/ (double) (camSet.height);
mPrinciplePointX = botConf.principlePointX;
mPrinciplePointY = botConf.principlePointY;
mCameraHeight = botConf.cameraHeight;
mCameraOffsetX = botConf.cameraOffsetX;
updateVector(getBall(), &getBallVector());
updateVectors( getGoalPoles().getObjects(), &getGoalPolesVectors().data);
updateVectors( getCyanRobots().getObjects(), &getCyanRobotsVectors().data);
updateVectors( getMagentaRobots().getObjects(), &getMagentaRobotsVectors().data);
updateVectors( getRobots().getObjects(), &getRobotsVectors().data);
return true;
}
Camera::Camera(glm::vec3* upVector, glm::vec3* initialPosition, glm::vec3* initialTarget)
: m_yaw(-90.0f)
, m_pitch(0.0f)
, m_mvtSpeed(3.0f)
, m_rotSpeed(0.25f)
, m_zoom(45.0f)
{
if (upVector)
m_worldUp = *upVector;
else
m_worldUp = glm::vec3(0, 1, 0);
if (initialPosition)
m_position = *initialPosition;
else
m_position = glm::vec3(0, 0, 0);
if (initialTarget)
m_target = *initialTarget;
else
m_target = glm::vec3(0, 0, 0);
m_zNeg = glm::normalize(m_position - m_target);
m_xPos = glm::normalize(glm::cross(m_worldUp, m_zNeg));
m_yPos = glm::cross(m_zNeg, m_xPos);
updateVectors();
}
void function::setNum(int _nNum)
{
if(_nNum<1)
return;
nNum=_nNum;
updateVectors();
}
GameObject::GameObject(float _positionX, float _positionY, float _positionZ, float _rotationX, float _rotationY, float _rotationZ, float _scaleX, float _scaleY, float _scaleZ)
: position( _positionX, _positionY, _positionZ), rotation( _rotationX, _rotationY, _rotationZ), scale( _scaleX, _scaleY, _scaleZ)
{
updateGridPosition();
transformed = true;
updated = true;
updateVectors();
}
//ALEX OWEN 20/04/15 - new constructor
GameObject::GameObject(XMFLOAT3 _position, XMFLOAT3 _rotation, XMFLOAT3 _scale)
: position(_position), rotation(_rotation), scale(_scale)
{
updateGridPosition();
transformed = true;
updated = true;
updateVectors();
}
void Camera::setLocation(const glm::vec3& location)
{
glm::vec3 to = m_target.value() - location;
m_dist = glm::length(to);
m_dx = atan2f(to[2], -to[0]);
m_dy = asinf(-to[1] / m_dist);
updateVectors();
}
void Camera::rotate(const glm::vec2& diff)
{
const glm::vec2 tmp = diff * 5.0f;
m_dx = m_dx.value() + tmp[0];
m_dy = m_dy.value() + tmp[1];
if (m_dy < -M_PI*0.499f) m_dy = -M_PI*0.499f;
if (m_dy > M_PI*0.499f) m_dy = M_PI*0.499f;
updateVectors();
}
bool GcodeDrawer::updateData()
{
switch (m_drawMode) {
case GcodeDrawer::Vectors:
if (m_indexes.isEmpty()) return prepareVectors(); else return updateVectors();
case GcodeDrawer::Raster:
if (m_indexes.isEmpty()) return prepareRaster(); else return updateRaster();
}
}
Camera::Camera(const QString& name)
: SceneObject(name),
m_type(*this, Perspective),
m_target(*this, glm::vec3(0, 0, 0)),
m_up(0, 1, 0),
m_dx(*this, 0),
m_dy(*this, 0),
m_dist(*this, 0.0f),
m_width(-1),
m_height(-1),
m_fov(*this, 45),
m_near(*this, 0.1f),
m_far(*this, 1000.0f)
{
updateVectors();
}
void Camera::processMouse(float xDelta, float yDelta)
{
xDelta *= mouseSensitivity;
yDelta *= mouseSensitivity;
yaw += xDelta;
pitch += yDelta;
if (pitch > 89.0f)
pitch = 89.0f;
if (pitch < -89.0f)
pitch = -89.0f;
updateVectors();
frustumDirty_ = true;
viewDirty_ = true;
}
void Camera::rotate(GLfloat xoffset, GLfloat yoffset, GLboolean constrainPitch /* = true */)
{
xoffset *= m_rotSpeed;
yoffset *= m_rotSpeed;
m_yaw += xoffset;
m_pitch += yoffset;
if (constrainPitch)
{
if (m_pitch > 89.0f)
m_pitch = 89.0f;
if (m_pitch < -89.0f)
m_pitch = -89.0f;
}
updateVectors();
}
void Camera::updateModelview()
{
_prevAngles = _angles;
_prevOrigin = _origin;
modelview = Matrix4::getIdentity();
// roll, pitch, yaw
Vector3 radiant_eulerXYZ(0, -_angles[CAMERA_PITCH], _angles[CAMERA_YAW]);
modelview.translateBy(_origin);
modelview.rotateByEulerXYZDegrees(radiant_eulerXYZ);
modelview.multiplyBy(g_radiant2opengl);
modelview.invert();
updateVectors();
m_view->Construct(projection, modelview, width, height);
}
//ALEX OWEN 30/03/15 access up Vector
XMVECTOR GameObject::getUpVector()
{
if(updated) updateVectors();
XMVECTOR _upVector = XMVectorSet(upVector.x, upVector.y, upVector.z, 0);
return _upVector;
}
//ALEX OWEN 30/03/15 access right Vector
XMVECTOR GameObject::getRightVector()
{
if(updated) updateVectors();
XMVECTOR _rightVector = XMVectorSet(rightVector.x, rightVector.y, rightVector.z, 0);
return _rightVector;
}
//ALEX OWEN 30/03/15 access forward Vector
XMVECTOR GameObject::getForwardVector()
{
if(updated) updateVectors();
XMVECTOR _forwardVector = XMVectorSet(forwardVector.x, forwardVector.y, forwardVector.z, 0);
return _forwardVector;
}
void FPSCamera::rotateYaw( float val ) {
_yaw += val * _rotateSensitivity;
_yaw = cc::math::wrapAngle(_yaw, 0.0f, 360.0f);
_viewDirty = true;
updateVectors();
}
void FPSCamera::rotatePitch( float val ) {
_pitch += val * _rotateSensitivity;
_pitch = cc::math::clamp(_pitch, -89.9f, 89.9f);
_viewDirty = true;
updateVectors();
}
void Camera::setType(Type type)
{
m_type = type;
updateVectors();
}
void FPSCamera::setPitch( float val ) {
_pitch = val;
_pitch = cc::math::clamp(_pitch, -89.9f, 89.9f);
_viewDirty = true;
updateVectors();
}
void FPSCamera::setYaw( float val ) {
_yaw = val;
_yaw = cc::math::wrapAngle(_yaw, 0.0f, 360.0f);
_viewDirty = true;
updateVectors();
}
