void Frustum::Enclose(const Frustum& other)
{
vec3 n = glm::normalize(other._origin - other._at);
vec3 u = glm::normalize(cross(other._up, n));
vec3 v = glm::normalize(cross(n, u));
if (_type == Projection::PERSPECTIVE)
Orient(_origin, other.GetCenter(), _up);
mat4 m = GetViewMatrix();
vec3 p[8];
if (other._type == Projection::PERSPECTIVE)
{
float dy = other._near * tanf(radians(other._fovy) / 2.0f);
float dx = other._ar * dy;
vec3 c = other._origin - n * other._near;
p[0] = c + u * dx + v * dy;
p[1] = c - u * dx + v * dy;
p[2] = c - u * dx - v * dy;
p[3] = c + u * dx - v * dy;
dy = other._far * tanf(glm::radians(other._fovy) / 2.0f);
dx = other._ar * dy;
c = other._origin - n * other._far;
p[4] = c + u * dx + v * dy;
p[5] = c - u * dx + v * dy;
p[6] = c - u * dx - v * dy;
p[7] = c + u * dx - v * dy;
}
else
{
vec3 c = other._origin - n * other._near;
p[0] = c + u * other._xmax + v * other._ymax;
p[1] = c + u * other._xmax + v * other._ymin;
p[2] = c + u * other._xmin + v * other._ymax;
p[3] = c + u * other._xmin + v * other._ymin;
c = other._origin - n * other._far;
p[4] = c + u * other._xmax + v * other._ymax;
p[5] = c + u * other._xmax + v * other._ymin;
p[6] = c + u * other._xmin + v * other._ymax;
p[7] = c + u * other._xmin + v * other._ymin;
}
if (_type == Projection::PERSPECTIVE)
{
_fovy = 0.0f;
_far = 0.0f;
_near = std::numeric_limits<float>::max();
float maxHorizAngle = 0.0f;
for (int i = 0; i < 8; i++)
{
vec4 pt = m * vec4(p[i], 1.0f);
if (pt.z < 0.0f)
{
float d = -pt.z;
float angle = atanf(fabs(pt.x) / d);
if (angle > maxHorizAngle)
maxHorizAngle = angle;
angle = glm::degrees(atanf(fabs(pt.y) / d));
if (angle * 2.0f > _fovy)
_fovy = angle * 2.0f;
if (_near > d)
_near = d;
if (_far < d)
_far = d;
}
}
float h = (_near * tanf(glm::radians(_fovy) / 2.0f))*2.0f;
float w = (_near * tanf(maxHorizAngle)) * 2.0f;
_ar = w / h;
}
else
{
_xmin = _ymin = _near = std::numeric_limits<float>::max();
_xmax = _ymax = _far = std::numeric_limits<float>::min();
for (int i = 0; i < 8; i++)
{
vec4 pt = m * vec4(p[i], 1.0f);
if (_xmin > pt.x)
_xmin = pt.x;
if (_xmax < pt.x)
_xmax = pt.x;
if (_ymin > pt.y)
_ymin = pt.y;
if (_ymax < pt.y)
_ymax = pt.y;
if (_near > -pt.z)
_near = -pt.z;
if (_far < -pt.z)
_far = -pt.z;
}
}
}
void Taxi::Update(float time, float seconds)
{
//m_position.y = sinf(time * (((m_speed + 1) / 13.0f) * 1.0f)) * 0.5f + 0.5f;
//m_position.y *= 0.5f;
sm::Vec3 oldPos = m_position;
if (IsOccupied())
{
m_timeLeft -= seconds;
if (m_timeLeft < 0.0f)
m_timeLeft = 0.0f;
}
sm::Vec3 turnPivot;
sm::Matrix turnMatrix;
float pivotDistance = 0.0f;
m_speed += m_acc * 5.0f * seconds;
static float maxSpeed = 14.0f;
if (m_acc == 0.0f)
{
m_speed -= MathUtils::Min(MathUtils::Abs(m_speed), 8.0f * seconds) * MathUtils::Sign(m_speed);
}
if (m_speed > 0.0f && m_acc == -1.0f)
{
m_speed -= MathUtils::Min(MathUtils::Abs(m_speed), 12.0f * seconds) * MathUtils::Sign(m_speed);
}
m_speed = MathUtils::Clamp(m_speed, -maxSpeed / 4, maxSpeed);
SoundManager::GetInstance()->SetEnginePitch((MathUtils::Abs(m_speed) / maxSpeed) * 1.0f + 1.0f);
m_wheelsAngle += 2.0f * m_turnValue * seconds;
m_wheelsAngle = MathUtils::Clamp(m_wheelsAngle, -MathUtils::PI4, MathUtils::PI4);
if (m_wheelsAngle != 0.0f)
{
if (m_wheelsAngle < 0.0)
{
pivotDistance = m_backFrontWheelsDistance / tanf(fabs(m_wheelsAngle));
turnPivot = sm::Vec3(m_baseBackRightWheelPosition.x + pivotDistance, 0, m_baseBackRightWheelPosition.z);
}
else
{
pivotDistance = m_backFrontWheelsDistance / tanf(fabs(m_wheelsAngle));
turnPivot = sm::Vec3(m_baseBackLeftWheelPosition.x - pivotDistance, 0, m_baseBackLeftWheelPosition.z);
}
float angleSpeed = m_speed / (2.0f * MathUtils::PI * MathUtils::Abs(turnPivot.x));
sm::Matrix turnMatrixNormal =
sm::Matrix::RotateAxisMatrix(
angleSpeed * (MathUtils::PI * 2.0f) * seconds * MathUtils::Sign(m_wheelsAngle),
0, 1, 0);
turnPivot = m_worldMatrix * turnPivot;
turnPivot.y = 0.0f;
turnMatrix =
sm::Matrix::TranslateMatrix(turnPivot) *
turnMatrixNormal *
sm::Matrix::TranslateMatrix(turnPivot.GetReversed());
sm::Vec3 prevCarDirection = m_carDirection;
m_carDirection = turnMatrixNormal * m_carDirection;
m_carDirection.Normalize();
float angleDiff = sm::Vec3::GetAngle(prevCarDirection, m_carDirection);
m_wheelsAngle -= angleDiff * MathUtils::Sign(m_wheelsAngle);
m_position = turnMatrix * m_position;
}
else
m_position += m_carDirection * m_speed * seconds;
sm::Matrix newWorldMatrix =
sm::Matrix::TranslateMatrix(m_position) *
sm::Matrix::CreateLookAt(m_carDirection.GetReversed(), sm::Vec3(0, 1, 0));
sm::Vec3 boundsTopLeftWorldOld = m_worldMatrix * m_boundsTopLeft;
sm::Vec3 boundsBottomLeftWorldOld = m_worldMatrix * m_boundsBottomLeft;
sm::Vec3 boundsTopRightWorldOld = m_worldMatrix * m_boundsTopRight;
sm::Vec3 boundsBottomRightWorldOld = m_worldMatrix * m_boundsBottomRight;
sm::Vec3 boundsTopLeftWorldNew = newWorldMatrix * m_boundsTopLeft;
sm::Vec3 boundsBottomLeftWorldNew = newWorldMatrix * m_boundsBottomLeft;
sm::Vec3 boundsTopRightWorldNew = newWorldMatrix * m_boundsTopRight;
sm::Vec3 boundsBottomRightWorldNew = newWorldMatrix * m_boundsBottomRight;
sm::Vec3 collisionNormal;
sm::Vec3 collisionPoint;
if (Street::Instance->GetCollistion(boundsTopLeftWorldOld, boundsTopLeftWorldNew, collisionPoint, collisionNormal))
{
if (m_speed > 4.0f)
{
static Randomizer random;
SoundManager::GetInstance()->PlaySound((SoundManager::Sound)random.GetInt(0, 2));
}
float dot = sm::Vec3::Dot(collisionNormal, m_carDirection.GetReversed());
if (dot > 0.8 && m_speed > 3.0f)
m_speed = -3.0f;
else
m_speed *= 1.0f - sm::Vec3::Dot(collisionNormal, m_carDirection.GetReversed());
collisionPoint += collisionNormal * 0.01f;
sm::Vec3 toCollisionTarget = collisionPoint - boundsTopLeftWorldOld;
sm::Vec3 fromCollisionTarget = sm::Vec3::Reflect(collisionNormal, toCollisionTarget);
sm::Vec3 correntPosition = boundsTopLeftWorldOld + toCollisionTarget + fromCollisionTarget;
sm::Vec3 deltaMove = collisionPoint - boundsTopLeftWorldOld;
m_carDirection = (collisionPoint - boundsBottomLeftWorldOld).GetNormalized();
m_position = oldPos + deltaMove;
m_worldMatrix =
sm::Matrix::TranslateMatrix(m_position) *
sm::Matrix::CreateLookAt(m_carDirection.GetReversed(), sm::Vec3(0, 1, 0));
}
else if (Street::Instance->GetCollistion(boundsTopRightWorldOld, boundsTopRightWorldNew, collisionPoint, collisionNormal))
{
if (m_speed > 4.0f)
{
static Randomizer random;
SoundManager::GetInstance()->PlaySound((SoundManager::Sound)random.GetInt(0, 2));
}
float dot = sm::Vec3::Dot(collisionNormal, m_carDirection.GetReversed());
if (dot > 0.8 && m_speed > 3.0f)
m_speed = -3.0f;
else
m_speed *= 1.0f - sm::Vec3::Dot(collisionNormal, m_carDirection.GetReversed());
collisionPoint += collisionNormal * 0.01f;
sm::Vec3 toCollisionTarget = collisionPoint - boundsTopRightWorldOld;
sm::Vec3 fromCollisionTarget = sm::Vec3::Reflect(collisionNormal, toCollisionTarget);
sm::Vec3 correntPosition = boundsTopRightWorldOld + toCollisionTarget + fromCollisionTarget;
sm::Vec3 deltaMove = collisionPoint - boundsTopRightWorldOld;
m_carDirection = (collisionPoint - boundsBottomRightWorldOld).GetNormalized();
m_position = oldPos + deltaMove;
m_worldMatrix =
sm::Matrix::TranslateMatrix(m_position) *
sm::Matrix::CreateLookAt(m_carDirection.GetReversed(), sm::Vec3(0, 1, 0));
}
/*else if (Street::Instance->GetCollistion(boundsBottomRightWorldOld, boundsBottomRightWorldNew, collisionPoint, collisionNormal))
{
if (m_speed > 4.0f)
{
static Randomizer random;
SoundManager::GetInstance()->PlaySound((SoundManager::Sound)random.GetInt(0, 2));
}
float dot = sm::Vec3::Dot(collisionNormal, m_carDirection.GetReversed());
if (dot > 0.8 && m_speed > 3.0f)
m_speed = -3.0f;
else
m_speed *= 1.0f - sm::Vec3::Dot(collisionNormal, m_carDirection.GetReversed());
collisionPoint += collisionNormal * 0.01f;
sm::Vec3 toCollisionTarget = collisionPoint - boundsBottomRightWorldOld;
sm::Vec3 fromCollisionTarget = sm::Vec3::Reflect(collisionNormal, toCollisionTarget);
sm::Vec3 correntPosition = boundsBottomRightWorldOld + toCollisionTarget + fromCollisionTarget;
sm::Vec3 deltaMove = collisionPoint - boundsBottomRightWorldOld;
m_carDirection = (boundsTopRightWorldOld - collisionPoint).GetNormalized();
m_position = oldPos + deltaMove;
m_worldMatrix =
sm::Matrix::TranslateMatrix(m_position) *
sm::Matrix::CreateLookAt(m_carDirection.GetReversed(), sm::Vec3(0, 1, 0));
}*/
else if (Street::Instance->GetCollistion(boundsBottomRightWorldOld, boundsBottomRightWorldNew, collisionPoint, collisionNormal))
{
if (m_speed > 4.0f)
{
static Randomizer random;
SoundManager::GetInstance()->PlaySound((SoundManager::Sound)random.GetInt(0, 2));
}
m_position = oldPos + collisionNormal * 0.1f;
m_speed = 0.0f;
m_worldMatrix =
sm::Matrix::TranslateMatrix(m_position) *
sm::Matrix::CreateLookAt(m_carDirection.GetReversed(), sm::Vec3(0, 1, 0));
}
else if (Street::Instance->GetCollistion(boundsBottomLeftWorldOld, boundsBottomLeftWorldNew, collisionPoint, collisionNormal))
{
if (m_speed > 4.0f)
{
static Randomizer random;
SoundManager::GetInstance()->PlaySound((SoundManager::Sound)random.GetInt(0, 2));
}
m_position = oldPos + collisionNormal * 0.1f;
m_speed = 0.0f;
m_worldMatrix =
sm::Matrix::TranslateMatrix(m_position) *
sm::Matrix::CreateLookAt(m_carDirection.GetReversed(), sm::Vec3(0, 1, 0));
}
else
m_worldMatrix = newWorldMatrix;
m_frontRightWheel->Transform() =
m_frontRightWheel->m_worldMatrix *
sm::Matrix::RotateAxisMatrix(m_wheelsAngle, 0, 1, 0) *
m_frontRightWheel->m_worldInverseMatrix;
m_frontLeftWheel->Transform() =
m_frontLeftWheel->m_worldMatrix *
sm::Matrix::RotateAxisMatrix(m_wheelsAngle, 0, 1, 0) *
m_frontLeftWheel->m_worldInverseMatrix;
}
void Frustum::enclose( const Frustum & other )
{
vec3 n = glm::normalize(other.origin - other.at);
vec3 u = glm::normalize(glm::cross(other.up, n));
vec3 v = glm::normalize(glm::cross(n, u));
if( type == Projection::PERSPECTIVE )
this->orient( origin, other.getCenter(), up );
mat4 m = this->getViewMatrix();
vec3 p[8];
// Get 8 points that define the frustum
if( other.type == Projection::PERSPECTIVE ) {
float dy = other.mNear * tanf( (float)TO_RADIANS(other.fovy) / 2.0f );
float dx = other.ar * dy;
vec3 c = other.origin - n * other.mNear;
p[0] = c + u * dx + v * dy;
p[1] = c - u * dx + v * dy;
p[2] = c - u * dx - v * dy;
p[3] = c + u * dx - v * dy;
dy = other.mFar * tanf( (float)TO_RADIANS(other.fovy) / 2.0f );
dx = other.ar * dy;
c = other.origin - n * other.mFar;
p[4] = c + u * dx + v * dy;
p[5] = c - u * dx + v * dy;
p[6] = c - u * dx - v * dy;
p[7] = c + u * dx - v * dy;
} else {
vec3 c = other.origin - n * other.mNear;
p[0] = c + u * other.xmax + v * other.ymax;
p[1] = c + u * other.xmax + v * other.ymin;
p[2] = c + u * other.xmin + v * other.ymax;
p[3] = c + u * other.xmin + v * other.ymin;
c = other.origin - n * other.mFar;
p[4] = c + u * other.xmax + v * other.ymax;
p[5] = c + u * other.xmax + v * other.ymin;
p[6] = c + u * other.xmin + v * other.ymax;
p[7] = c + u * other.xmin + v * other.ymin;
}
// Adjust frustum to contain
if( type == Projection::PERSPECTIVE ) {
fovy = 0.0f;
mFar = 0.0f;
mNear = std::numeric_limits<float>::max();
float maxHorizAngle = 0.0f;
for( int i = 0; i < 8; i++) {
// Convert to local space
vec4 pt = m * vec4(p[i],1.0f);
if( pt.z < 0.0f ) {
float d = -pt.z;
float angle = atanf( fabs(pt.x) / d );
if( angle > maxHorizAngle ) maxHorizAngle = angle;
angle = (float)TO_DEGREES( atanf( fabs(pt.y) / d ) );
if( angle * 2.0f > fovy ) fovy = angle * 2.0f;
if( mNear > d ) mNear = d;
if( mFar < d ) mFar = d;
}
}
float h = ( mNear * tanf( (float)TO_RADIANS(fovy)/ 2.0f) ) * 2.0f;
float w = ( mNear * tanf( maxHorizAngle ) ) * 2.0f;
ar = w / h;
} else {
xmin = ymin = mNear = std::numeric_limits<float>::max();
xmax = ymax = mFar = std::numeric_limits<float>::min();
for( int i = 0; i < 8; i++) {
// Convert to local space
vec4 pt = m * vec4(p[i],1.0f);
if( xmin > pt.x ) xmin = pt.x;
if( xmax < pt.x ) xmax = pt.x;
if( ymin > pt.y ) ymin = pt.y;
if( ymax < pt.y ) ymax = pt.y;
if( mNear > -pt.z ) mNear = -pt.z;
if( mFar < -pt.z ) mFar = -pt.z;
}
}
}
void qKinect::grabCloud()
{
assert(f_ctx && f_dev);
assert(m_kDlg);
if (m_timer)
{
m_timer->stop();
QApplication::processEvents();
}
bool grabRGB = m_kDlg->grabRGBInfo();
unsigned char framesAvgCount = m_kDlg->getFrameAveragingCount();
uint16_t* old_depth_data = s_depth_data;
const unsigned mapSize = s_wDepth*s_hDepth;
if (framesAvgCount>1)
{
uint16_t* depth_frames = new uint16_t[mapSize*framesAvgCount];
if (!depth_frames)
{
m_app->dispToConsole("[qKinect] Not enough memory to compute frame averaging!");
framesAvgCount=1;
}
else
{
s_depth_data = depth_frames;
}
}
//Flush buffers
s_max_depth_count = 0;
s_depth_count = 1;
s_rgb_count = 1;
freenect_process_events(f_ctx);
//Start
QElapsedTimer eTimer;
eTimer.start();
s_rgb_count = 0;
s_depth_count = 0;
s_max_depth_count = framesAvgCount;
while (s_depth_count<framesAvgCount || (grabRGB && s_rgb_count==0))
{
freenect_process_events(f_ctx);
if(s_depth_count == 0 && eTimer.elapsed() > 5000 ) //timeout 5s. without any data
break;
}
//success?
if (s_depth_count)
{
const unsigned mapSize = s_hDepth*s_wDepth;
//first, pocess frame averaging (if any)
if (s_depth_count>1)
{
const unsigned char minFrameCount = (framesAvgCount>>1); //a pixel must be visible in at least half the frames!
uint16_t *_depth = s_depth_data;
for (unsigned i = 0; i < mapSize; ++i,++_depth)
{
//sum all equivalent pixels
unsigned pixCount = 0;
double pixSum = 0.0;
const uint16_t* kDepth = s_depth_data + i;
for (unsigned k=0;k<s_depth_count;++k,kDepth+=mapSize)
{
if (*kDepth < FREENECT_DEPTH_RAW_NO_VALUE)
{
pixSum += (double)s_depth_data[k*mapSize + i];
++pixCount;
}
}
if (pixCount>=minFrameCount)
{
*_depth = (uint16_t)(pixSum/(double)pixCount);
}
else
{
*_depth = FREENECT_DEPTH_RAW_NO_VALUE;
}
}
}
/*** Depth calibration info ***/
//see http://openkinect.org/wiki/Imaging_Information
/*static const float minDistance = -10.0f;
static const float scaleFactor = .0021f;
const float cx = (float)w/2;
const float cy = (float)h/2;
const float fx = 1.0f/scaleFactor; //* (480.0/640.0);
const float fy = 1.0f/scaleFactor; //~476
//*/
//see http://nicolas.burrus.name/index.php/Research/KinectCalibration
static const float minDistance = -10.0f;
static const float cx = 339.5f;
static const float cy = 242.7f;
static const float fx = 594.21f;
static const float fy = 591.04f;
//*/
/*** RGB calibration info ***/
static const float fx_rgb = 529.21508098293293f;
static const float fy_rgb = 525.56393630057437f;
static const float cx_rgb = 328.94272028759258f;
static const float cy_rgb = 267.48068171871557f;
float mat[16]={9.9984628826577793e-01f, 1.2635359098409581e-03f, -1.7487233004436643e-02f, 1.9985242312092553e-02f,
-1.4779096108364480e-03f, 9.9992385683542895e-01f, -1.2251380107679535e-02f, -7.4423738761617583e-04f,
1.7470421412464927e-02f, 1.2275341476520762e-02f, 9.9977202419716948e-01f, -1.0916736334336222e-02f,
0.0f, 0.0f, 0.0f, 1.0f};
ccGLMatrix depth2rgb(mat);
depth2rgb.transpose();
ccPointCloud* depthMap = new ccPointCloud();
bool hasRGB = s_rgb_count && s_last_rgb_data && m_kDlg->grabRGBCheckBox->isChecked();
if (depthMap->reserve(mapSize))
{
if (hasRGB)
{
if (depthMap->reserveTheRGBTable())
depthMap->showColors(true);
else
{
m_app->dispToConsole("[qKinect] Not enough memory to grab colors!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
hasRGB=false;
}
}
const uint16_t *depth = s_depth_data;
const uint8_t* col = 0;
const uint8_t white[3]={255,255,255};
CCVector3 P,Q;
bool meanFilter = m_kDlg->meanFilterCheckBox->isChecked();
for (unsigned j=0; j<s_hDepth; ++j)
{
//P.y = (PointCoordinateType)j;
for (unsigned i=0; i<s_wDepth; ++i,++depth)
{
uint16_t d = *depth;
//mean filter
if (meanFilter)
{
double sum = 0.0;
unsigned count = 0;
for (int k=-1; k<=1; ++k)
{
int ii = static_cast<int>(i)+k;
if (ii>=0 && ii<static_cast<int>(s_wDepth))
for (int l=-1;l<=1;++l)
{
int jj = static_cast<int>(j)+l;
if (jj>=0 && jj<static_cast<int>(s_hDepth))
{
const uint16_t& dd = s_depth_data[jj*s_wDepth+ii];
if (dd < FREENECT_DEPTH_RAW_NO_VALUE)
{
sum += static_cast<double>(s_depth_data[jj*s_wDepth+ii]);
++count;
}
}
}
}
if (count > 1)
d = static_cast<uint16_t>(sum/count);
}
if (d < FREENECT_DEPTH_RAW_NO_VALUE)
{
//see http://openkinect.org/wiki/Imaging_Information
P.z = 12.36f * tanf(static_cast<float>(d) / 2842.5f + 1.1863f) - 3.7f;
//see http://nicolas.burrus.name/index.php/Research/KinectCalibration
P.x = (static_cast<float>(i) - cx) * (P.z + minDistance) / fx;
P.y = (static_cast<float>(j) - cy) * (P.z + minDistance) / fy ;
if (hasRGB)
{
assert(s_last_rgb_data);
Q = depth2rgb * P;
Q.x = (Q.x * fx_rgb / Q.z) + cx_rgb;
Q.y = (Q.y * fy_rgb / Q.z) + cy_rgb;
int i_rgb = (int)Q.x;
int j_rgb = (int)Q.y;
if (i_rgb>=0 && i_rgb<(int)s_wDepth && j_rgb>=0 && j_rgb<(int)s_hDepth)
{
col = s_last_rgb_data+(i_rgb+j_rgb*s_wRgb)*3;
}
else
{
col = white;
}
}
P.y = -P.y;
P.z = -P.z;
depthMap->addPoint(P);
if (col)
depthMap->addRGBColor(col);
}
}
}
//m_app->dispToConsole(QString("Cloud captured: %1 points").arg(depthMap->size()));
depthMap->resize(depthMap->size());
QString cloudName = m_kDlg->getCloudName() + QString::number(++s_grabIndex);
depthMap->setName(qPrintable(cloudName));
//associate sensor
ccGBLSensor* sensor = new ccGBLSensor(ccGBLSensor::THETA_PHI);
ccGLMatrix rot;
{
float* mat = rot.data();
mat[0] = 1.0f;
mat[1] = 0.0f;
mat[2] = 0.0f;
mat[4] = 0.0f;
mat[5] = 0.0f;
mat[6] = -1.0f;
mat[8] = 0.0f;
mat[9] = 1.0f;
mat[10] = 0.0f;
mat[15] = 1.0f;
}
sensor->setRigidTransformation(rot);
sensor->setDeltaPhi(0.0017f);
sensor->setDeltaTheta(0.0017f);
sensor->setUncertainty(1e-3f);
{
int errorCode = 0;
CCLib::SimpleCloud* cloud = sensor->project(depthMap,errorCode,true);
if (cloud)
delete cloud;
cloud = 0;
if (errorCode == 0)
{
sensor->setName("Kinect");
sensor->setGraphicScale(20.0f);
sensor->setVisible(true);
depthMap->addChild(sensor);
}
else
{
delete sensor;
sensor = 0;
}
}
//selectedEntities.push_back(depthMap);
m_app->addToDB(depthMap,false,true,true);
m_app->refreshAll();
}
else
{
//not enough memory
delete depthMap;
depthMap = 0;
//result = -5;
}
}
else
{
//no data!
//result = -6;
}
static void node_shader_exec_math(void *UNUSED(data), int UNUSED(thread), bNode *node, bNodeExecData *UNUSED(execdata), bNodeStack **in, bNodeStack **out)
{
float a, b, r = 0.0f;
nodestack_get_vec(&a, SOCK_FLOAT, in[0]);
nodestack_get_vec(&b, SOCK_FLOAT, in[1]);
switch (node->custom1) {
case 0: /* Add */
r = a + b;
break;
case 1: /* Subtract */
r = a - b;
break;
case 2: /* Multiply */
r = a * b;
break;
case 3: /* Divide */
{
if (b == 0) /* We don't want to divide by zero. */
r = 0.0;
else
r = a / b;
break;
}
case 4: /* Sine */
{
if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
r = sinf(a);
else
r = sinf(b);
break;
}
case 5: /* Cosine */
{
if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
r = cosf(a);
else
r = cosf(b);
break;
}
case 6: /* Tangent */
{
if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
r = tanf(a);
else
r = tanf(b);
break;
}
case 7: /* Arc-Sine */
{
if (in[0]->hasinput || !in[1]->hasinput) { /* This one only takes one input, so we've got to choose. */
/* Can't do the impossible... */
if (a <= 1 && a >= -1)
r = asinf(a);
else
r = 0.0;
}
else {
/* Can't do the impossible... */
if (b <= 1 && b >= -1)
r = asinf(b);
else
r = 0.0;
}
break;
}
case 8: /* Arc-Cosine */
{
if (in[0]->hasinput || !in[1]->hasinput) { /* This one only takes one input, so we've got to choose. */
/* Can't do the impossible... */
if (a <= 1 && a >= -1)
r = acosf(a);
else
r = 0.0;
}
else {
/* Can't do the impossible... */
if (b <= 1 && b >= -1)
r = acosf(b);
else
r = 0.0;
}
break;
}
case 9: /* Arc-Tangent */
{
if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
r = atan(a);
else
r = atan(b);
break;
}
case 10: /* Power */
{
/* Only raise negative numbers by full integers */
if (a >= 0) {
r = pow(a, b);
}
else {
float y_mod_1 = fabsf(fmodf(b, 1.0f));
/* if input value is not nearly an integer, fall back to zero, nicer than straight rounding */
if (y_mod_1 > 0.999f || y_mod_1 < 0.001f) {
r = powf(a, floorf(b + 0.5f));
}
else {
r = 0.0f;
}
}
break;
}
case 11: /* Logarithm */
{
/* Don't want any imaginary numbers... */
if (a > 0 && b > 0)
r = log(a) / log(b);
else
r = 0.0;
break;
}
case 12: /* Minimum */
{
if (a < b)
r = a;
else
r = b;
break;
}
case 13: /* Maximum */
{
if (a > b)
r = a;
else
r = b;
break;
}
case 14: /* Round */
{
if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
r = (a < 0) ? (int)(a - 0.5f) : (int)(a + 0.5f);
else
r = (b < 0) ? (int)(b - 0.5f) : (int)(b + 0.5f);
break;
}
case 15: /* Less Than */
{
if (a < b)
r = 1.0f;
else
r = 0.0f;
break;
}
case 16: /* Greater Than */
{
if (a > b)
r = 1.0f;
else
r = 0.0f;
break;
}
case 17: /* Modulo */
{
if (b == 0.0f)
r = 0.0f;
else
r = fmod(a, b);
break;
}
case 18: /* Absolute */
{
r = fabsf(a);
break;
}
}
out[0]->vec[0] = r;
}
//----------------------------------------------------------
void ofCairoRenderer::setupScreenPerspective(float width, float height, float fov, float nearDist, float farDist){
if(!b3D) return;
if(width < 0) width = viewportRect.width;
if(height < 0) height = viewportRect.height;
ofOrientation orientation = ofGetOrientation();
float viewW = viewportRect.width;
float viewH = viewportRect.height;
float eyeX = viewW / 2;
float eyeY = viewH / 2;
float halfFov = PI * fov / 360;
float theTan = tanf(halfFov);
float dist = eyeY / theTan;
float aspect = (float) viewW / viewH;
if(nearDist == 0) nearDist = dist / 10.0f;
if(farDist == 0) farDist = dist * 10.0f;
projection.makePerspectiveMatrix(fov,aspect,nearDist,farDist);
modelView.makeLookAtViewMatrix(ofVec3f(eyeX,eyeY,dist),ofVec3f(eyeX,eyeY,0),ofVec3f(0,1,0));
//note - theo checked this on iPhone and Desktop for both vFlip = false and true
switch(orientation) {
case OF_ORIENTATION_180:
modelView.glRotate(-180,0,0,1);
if(isVFlipped()){
modelView.glScale(-1,-1,1);
modelView.glTranslate(width,0,0);
}else{
modelView.glTranslate(width,-height,0);
}
break;
case OF_ORIENTATION_90_RIGHT:
modelView.glRotate(-90,0,0,1);
if(isVFlipped()){
modelView.glScale(1,1,1);
}else{
modelView.glScale(1,-1,1);
modelView.glTranslate(-width,-height,0);
}
break;
case OF_ORIENTATION_90_LEFT:
modelView.glRotate(90,0,0,1);
if(isVFlipped()){
modelView.glScale(1,1,1);
modelView.glTranslate(0,-height,0);
}else{
modelView.glScale(1,-1,1);
modelView.glTranslate(0,0,0);
}
break;
case OF_ORIENTATION_DEFAULT:
default:
if(isVFlipped()){
modelView.glScale(-1,-1,1);
modelView.glTranslate(-width,-height,0);
}
break;
}
};
// Add the weapon, and flash for the player's view
void CG_AddViewWeapon( playerState_t *ps ) {
// no gun if in third person view or a camera is active
if ( ps->persistant[PERS_TEAM] == TEAM_SPECTATOR || ps->pm_type == PM_INTERMISSION || cg.renderingThirdPerson ) {
return;
}
const int weap = cg_fakeGun.integer ? cg_fakeGun.integer : ps->weapon;
float desiredFov = 0.0f;
if ( !cg.renderingThirdPerson
&& (cg_trueGuns.integer || weap == WP_SABER || weap == WP_MELEE)
&& cg_trueFOV.value
&& cg.predictedPlayerState.pm_type != PM_SPECTATOR
&& cg.predictedPlayerState.pm_type != PM_INTERMISSION )
{
desiredFov = cg_fovViewmodel.integer ? cg_fovViewmodel.value : cg_trueFOV.value;
}
else {
desiredFov = cg_fovViewmodel.integer ? cg_fovViewmodel.value : cg_fov.value;
}
desiredFov = Q_clampi( 1, desiredFov, 180 );
// allow the gun to be completely removed
if ( !cg_fakeGun.integer && (!cg_drawGun.integer || cg.predictedPlayerState.zoomMode || cg_trueGuns.integer
|| weap == WP_SABER || weap == WP_MELEE) ) {
return;
}
// don't draw if testing a gun model
if ( cg.testGun ) {
return;
}
centity_t *cent = &cg_entities[cg.predictedPlayerState.clientNum];
CG_RegisterWeapon( weap );
refEntity_t hand;
memset( &hand, 0, sizeof(hand) );
// set up gun position
vector3 angles;
CG_CalculateWeaponPosition( &hand.origin, &angles );
refdef_t *refdef = CG_GetRefdef();
VectorMA( &hand.origin, cg.gunAlign.x, &refdef->viewaxis[0], &hand.origin );
VectorMA( &hand.origin, cg.gunAlign.y, &refdef->viewaxis[1], &hand.origin );
VectorMA( &hand.origin, cg.gunAlign.z, &refdef->viewaxis[2], &hand.origin );
AnglesToAxis( &angles, hand.axis );
if ( cg_fovViewmodel.integer ) {
float fracDistFOV, fracWeapFOV;
float fov = desiredFov;
if ( cg_fovAspectAdjust.integer ) {
// Based on LordHavoc's code for Darkplaces
// http://www.quakeworld.nu/forum/topic/53/what-does-your-qw-look-like/page/30
const float baseAspect = 0.75f; // 3/4
const float aspect = (float)cgs.glconfig.vidWidth / (float)cgs.glconfig.vidHeight;
fov = atanf( tanf( desiredFov*M_PI / 360.0f ) * baseAspect*aspect )*360.0f / M_PI;
}
fracDistFOV = tanf( refdef->fov_x * M_PI / 360.0f );
fracWeapFOV = (1.0f / fracDistFOV) * tanf( fov * M_PI / 360.0f );
VectorScale( &hand.axis[0], fracWeapFOV, &hand.axis[0] );
}
// map torso animations to weapon animations
if ( cg_debugGunFrame.integer ) {
// development tool
hand.frame = hand.oldframe = cg_debugGunFrame.integer;
hand.backlerp = 0;
}
else {
float currentFrame;
// get clientinfo for animation map
clientInfo_t *ci = nullptr;
if ( cent->currentState.eType == ET_NPC ) {
if ( !cent->npcClient ) {
return;
}
ci = cent->npcClient;
}
else {
ci = &cgs.clientinfo[cent->currentState.clientNum];
}
// smoother first-person anims by eezstreet http://jkhub.org/topic/1499-/
// actually ported from SP
#if 1
// Sil's fix
trap->G2API_GetBoneFrame( cent->ghoul2, "lower_lumbar", cg.time, ¤tFrame, cgs.gameModels, 0 );
hand.frame = CG_MapTorsoToWeaponFrame( ci, ceilf( currentFrame ), cent->currentState.torsoAnim );
hand.oldframe = CG_MapTorsoToWeaponFrame( ci, floorf( currentFrame ), cent->currentState.torsoAnim );
hand.backlerp = 1.0f - (currentFrame - floorf( currentFrame ));
#else
// basejka style
hand.frame = CG_MapTorsoToWeaponFrame( ci, cent->pe.torso.frame, cent->currentState.torsoAnim );
hand.oldframe = CG_MapTorsoToWeaponFrame( ci, cent->pe.torso.oldFrame, cent->currentState.torsoAnim );
hand.backlerp = cent->pe.torso.backlerp;
#endif
// Handle the fringe situation where oldframe is invalid
if ( hand.frame == -1 ) {
hand.frame = 0;
hand.oldframe = 0;
hand.backlerp = 0;
}
else if ( hand.oldframe == -1 ) {
hand.oldframe = hand.frame;
hand.backlerp = 0;
}
}
weaponInfo_t *wi = &cg_weapons[weap];
hand.hModel = wi->handsModel;
hand.renderfx = RF_DEPTHHACK | RF_FIRST_PERSON;
// add everything onto the hand
CG_AddPlayerWeapon( &hand, ps, &cg_entities[cg.predictedPlayerState.clientNum], ps->persistant[PERS_TEAM], &angles, qfalse );
}
//-------------------------------------------------------------
//! @brief : タンジェント
//! @param[in] : ラジアン値(F32)
//! @return : タンジェント値
//-------------------------------------------------------------
F32 Tan( F32 rad )
{
return tanf(rad);
}
float ECL_PitchController::control_bodyrate(const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
/* apply turning offset to desired bodyrate setpoint*/
/* flying inverted (wings upside down)*/
bool inverted = false;
float constrained_roll;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
} else {
/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
inverted = true;
/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
if (ctl_data.roll > 0.0f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
} else {
/* left hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
}
}
/* input conditioning */
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
Availible on google books 8/11/2015:
https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(constrained_roll) * sinf(constrained_roll)));
if (inverted) {
body_fixed_turn_offset = -body_fixed_turn_offset;
}
/* Finally add the turn offset to your bodyrate setpoint*/
_bodyrate_setpoint += body_fixed_turn_offset;
_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
if (!lock_integrator && _k_i > 0.0f) {
float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
_dif_rate_error = _rate_error - _last_rate_error;
_last_rate_error = _rate_error;
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
fp = open(PX4_ROOTFSDIR"/fs/microsd/log/output_pitch.csv", O_CREAT | O_WRONLY | O_DSYNC | O_APPEND);
int bytes = sprintf(buffer, "%.6f,%.6f,%.6f\n", (double)_rate_error, (double)_dif_rate_error, (double)math::constrain(_last_output, -1.0f, 1.0f));
write(fp, buffer, bytes);
close(fp);
return math::constrain(_last_output, -1.0f, 1.0f);
}
//----------------------------------------------------------------//
float Cot ( float radians ) {
return 1.0f / tanf ( radians );
}
//----------------------------------------------------------------//
float Tan ( float radians ) {
return tanf ( radians );
}
const kmMat4& Node::getNodeToParentTransform() const
{
if (_transformDirty)
{
// Translate values
float x = _position.x;
float y = _position.y;
float z = _positionZ;
if (_ignoreAnchorPointForPosition)
{
x += _anchorPointInPoints.x;
y += _anchorPointInPoints.y;
}
// Rotation values
// Change rotation code to handle X and Y
// If we skew with the exact same value for both x and y then we're simply just rotating
float cx = 1, sx = 0, cy = 1, sy = 0;
if (_rotationZ_X || _rotationZ_Y)
{
float radiansX = -CC_DEGREES_TO_RADIANS(_rotationZ_X);
float radiansY = -CC_DEGREES_TO_RADIANS(_rotationZ_Y);
cx = cosf(radiansX);
sx = sinf(radiansX);
cy = cosf(radiansY);
sy = sinf(radiansY);
}
bool needsSkewMatrix = ( _skewX || _skewY );
// optimization:
// inline anchor point calculation if skew is not needed
// Adjusted transform calculation for rotational skew
if (! needsSkewMatrix && !_anchorPointInPoints.equals(Point::ZERO))
{
x += cy * -_anchorPointInPoints.x * _scaleX + -sx * -_anchorPointInPoints.y * _scaleY;
y += sy * -_anchorPointInPoints.x * _scaleX + cx * -_anchorPointInPoints.y * _scaleY;
}
// Build Transform Matrix
// Adjusted transform calculation for rotational skew
kmScalar mat[] = {
cy * _scaleX, sy * _scaleX, 0, 0,
-sx * _scaleY, cx * _scaleY, 0, 0,
0, 0, _scaleZ, 0,
x, y, z, 1 };
kmMat4Fill(&_transform, mat);
// XXX
// FIX ME: Expensive operation.
// FIX ME: It should be done together with the rotationZ
if(_rotationY) {
kmMat4 rotY;
kmMat4RotationY(&rotY,CC_DEGREES_TO_RADIANS(_rotationY));
kmMat4Multiply(&_transform, &_transform, &rotY);
}
if(_rotationX) {
kmMat4 rotX;
kmMat4RotationX(&rotX,CC_DEGREES_TO_RADIANS(_rotationX));
kmMat4Multiply(&_transform, &_transform, &rotX);
}
// XXX: Try to inline skew
// If skew is needed, apply skew and then anchor point
if (needsSkewMatrix)
{
kmMat4 skewMatrix = { 1, (float)tanf(CC_DEGREES_TO_RADIANS(_skewY)), 0, 0,
(float)tanf(CC_DEGREES_TO_RADIANS(_skewX)), 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1};
kmMat4Multiply(&_transform, &_transform, &skewMatrix);
// adjust anchor point
if (!_anchorPointInPoints.equals(Point::ZERO))
{
// XXX: Argh, kmMat needs a "translate" method.
// XXX: Although this is faster than multiplying a vec4 * mat4
_transform.mat[12] += _transform.mat[0] * -_anchorPointInPoints.x + _transform.mat[4] * -_anchorPointInPoints.y;
_transform.mat[13] += _transform.mat[1] * -_anchorPointInPoints.x + _transform.mat[5] * -_anchorPointInPoints.y;
}
}
if (_useAdditionalTransform)
{
kmMat4Multiply(&_transform, &_transform, &_additionalTransform);
}
_transformDirty = false;
}
return _transform;
}
int main(int argc, char** argv)
{
GLFWwindow* window;
int iter;
double dt;
double last_update_time;
int frame;
float f;
GLint uloc_modelview;
GLint uloc_project;
GLuint shader_program;
glfwSetErrorCallback(error_callback);
if (!glfwInit())
exit(EXIT_FAILURE);
glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
window = glfwCreateWindow(800, 600, "GLFW OpenGL3 Heightmap demo", NULL, NULL);
if (! window )
{
glfwTerminate();
exit(EXIT_FAILURE);
}
/* Register events callback */
glfwSetKeyCallback(window, key_callback);
glfwMakeContextCurrent(window);
gladLoadGLLoader((GLADloadproc) glfwGetProcAddress);
/* Prepare opengl resources for rendering */
shader_program = make_shader_program(vertex_shader_text, fragment_shader_text);
if (shader_program == 0u)
{
glfwTerminate();
exit(EXIT_FAILURE);
}
glUseProgram(shader_program);
uloc_project = glGetUniformLocation(shader_program, "project");
uloc_modelview = glGetUniformLocation(shader_program, "modelview");
/* Compute the projection matrix */
f = 1.0f / tanf(view_angle / 2.0f);
projection_matrix[0] = f / aspect_ratio;
projection_matrix[5] = f;
projection_matrix[10] = (z_far + z_near)/ (z_near - z_far);
projection_matrix[11] = -1.0f;
projection_matrix[14] = 2.0f * (z_far * z_near) / (z_near - z_far);
glUniformMatrix4fv(uloc_project, 1, GL_FALSE, projection_matrix);
/* Set the camera position */
modelview_matrix[12] = -5.0f;
modelview_matrix[13] = -5.0f;
modelview_matrix[14] = -20.0f;
glUniformMatrix4fv(uloc_modelview, 1, GL_FALSE, modelview_matrix);
/* Create mesh data */
init_map();
make_mesh(shader_program);
/* Create vao + vbo to store the mesh */
/* Create the vbo to store all the information for the grid and the height */
/* setup the scene ready for rendering */
glViewport(0, 0, 800, 600);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
/* main loop */
frame = 0;
iter = 0;
last_update_time = glfwGetTime();
while (!glfwWindowShouldClose(window))
{
++frame;
/* render the next frame */
glClear(GL_COLOR_BUFFER_BIT);
glDrawElements(GL_LINES, 2* MAP_NUM_LINES , GL_UNSIGNED_INT, 0);
/* display and process events through callbacks */
glfwSwapBuffers(window);
glfwPollEvents();
/* Check the frame rate and update the heightmap if needed */
dt = glfwGetTime();
if ((dt - last_update_time) > 0.2)
{
/* generate the next iteration of the heightmap */
if (iter < MAX_ITER)
{
update_map(NUM_ITER_AT_A_TIME);
update_mesh();
iter += NUM_ITER_AT_A_TIME;
}
last_update_time = dt;
frame = 0;
}
}
glfwTerminate();
exit(EXIT_SUCCESS);
}
bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) {
// set target speed for approach on final
if (init){
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed;
#endif
init = FALSE;
}
// calculate distance tod_td
float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
struct EnuCoor_f* pos_enu = stateGetPositionEnu_f();
float hspeed = *stateGetHorizontalSpeedNorm_f();
float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
(pos_enu->y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress,-1,1);
// float nav_final_length = sqrt(final2);
// calculate requiered decent rate on glideslope
float pre_climb_glideslope = hspeed * (-tanf(app_angle));
// calculate glideslope
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt_glideslope = start_alt + nav_final_progress * diff_alt;
// calculate intercept
float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2*sd_tod_x +
(pos_enu->y - WaypointY(_sd)) * 2*sd_tod_y) /
Max((sd_intercept*sd_intercept),1.);
Bound(nav_intercept_progress,-1,1);
float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));
//########################################################
// handle the different vertical approach segments
float pre_climb = 0.;
float alt = 0.;
// distance
float f_af = sqrt((pos_enu->x - WaypointX(_af))*(pos_enu->x - WaypointX(_af))+
(pos_enu->y - WaypointY(_af))*(pos_enu->y - WaypointY(_af)));
if(f_af<app_distance_af_sd){ // approach fix (AF) to start descent (SD)
alt = WaypointAlt(_af);
pre_climb = 0.;
}
else if((f_af>app_distance_af_sd) && (f_af<(app_distance_af_sd+sd_intercept))){
// start descent (SD) to intercept
alt = alt_intercept;
pre_climb = pre_climb_intercept;
}
else{ //glideslope (intercept to touch down)
alt = alt_glideslope;
pre_climb = pre_climb_glideslope;
}
// Bound(pre_climb, -5, 0.);
//######################### autopilot modes
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
return TRUE;
} // end of gls()
float Angle::getTangent() const {
return tanf(getRadians());
}
void Functions::tan(Aurora::NWScript::FunctionContext &ctx) {
ctx.getReturn() = tanf(ctx.getParams()[0].getFloat());
}
void R_VR_GenerateHud()
{
int i, j;
float numsegments = floor(vr_hud_segments->value);
float horizFOV = vr_hud_fov->value;
float depth = vr_hud_depth->value;
float horizInterval = 2.0 / numsegments;
float vertBounds = (float) hud.height / (float) hud.width;
float vertInterval = horizInterval * vertBounds;
int numindices = (numsegments) * (numsegments + 1) * 2 + (numsegments * 2);
int numverts = (numsegments) * (numsegments + 1) * 2;
vert_t *hudverts = NULL;
GLushort *indices = NULL;
uint32_t hudNumVerts = 0;
GLushort currIndex = 0;
uint32_t iboSize = sizeof(GLushort) * numindices;
uint32_t vboSize = sizeof(vert_t) * numverts;
// calculate coordinates for hud
float xoff = tanf(horizFOV * (M_PI / 180.0f) * 0.5) * (depth);
float zoff = depth * cosf(horizFOV * (M_PI / 180.0f) * 0.5);
vec3_t offsetScale;
VectorSet(offsetScale, xoff, xoff, zoff);
hudverts = (vert_t *) malloc(vboSize);
memset(hudverts, 0, vboSize);
indices = (GLushort *) malloc(iboSize);
memset(indices, 0, iboSize);
for (j = 0; j < numsegments; j++)
{
float ypos, ypos1;
qboolean verticalHalf = (j >= numsegments / 2);
ypos = j * vertInterval - vertBounds;
ypos1 = (j + 1) * vertInterval - vertBounds;
for (i = 0; i <= numsegments; i++)
{
float xpos;
vert_t vert1, vert2;
GLushort vertNum1, vertNum2;
qboolean horizontalHalf = (i >= (numsegments+1) / 2);
xpos = i * horizInterval - 1;
VectorSet(vert1.position, xpos, ypos, -1);
sphereProject(vert1.position, offsetScale, vert1.position);
vert1.texCoords[0] = (float) i / (float) (numsegments);
vert1.texCoords[1] = (float) j / (float) (numsegments);
VectorSet(vert2.position, xpos, ypos1, -1);
sphereProject(vert2.position, offsetScale, vert2.position);
vert2.texCoords[0] = (float) i / (float) (numsegments);
vert2.texCoords[1] = (float) (j + 1) / (float) (numsegments);
vertNum1 = hudNumVerts++;
vertNum2 = hudNumVerts++;
if (verticalHalf)
{
hudverts[vertNum2] = vert1;
hudverts[vertNum1] = vert2;
} else {
hudverts[vertNum1] = vert1;
hudverts[vertNum2] = vert2;
}
if (j > 0 && i == 0)
{
indices[currIndex++] = vertNum1;
}
indices[currIndex++] = vertNum1;
indices[currIndex++] = vertNum2;
if (i == numsegments && j < (numsegments - 1))
{
indices[currIndex++] = vertNum2;
}
}
}
R_BindIVBO(&hudVBO,NULL,0);
R_VertexData(&hudVBO,hudNumVerts * sizeof(vert_t),hudverts);
R_IndexData(&hudVBO,GL_TRIANGLE_STRIP,GL_UNSIGNED_SHORT,currIndex,currIndex * sizeof(GLushort),indices);
R_ReleaseIVBO();
free(hudverts);
free(indices);
}
float stans(float x) {
return (tanf(x));
}
void Font::drawGlyphs(GraphicsContext* graphicsContext, const SimpleFontData* font, const GlyphBuffer& glyphBuffer,
int from, int numGlyphs, const FloatPoint& point) const
{
CGContextRef cgContext = graphicsContext->platformContext();
bool shouldUseFontSmoothing = WebCoreShouldUseFontSmoothing();
switch(fontDescription().fontSmoothing()) {
case Antialiased: {
graphicsContext->setShouldAntialias(true);
shouldUseFontSmoothing = false;
break;
}
case SubpixelAntialiased: {
graphicsContext->setShouldAntialias(true);
shouldUseFontSmoothing = true;
break;
}
case NoSmoothing: {
graphicsContext->setShouldAntialias(false);
shouldUseFontSmoothing = false;
break;
}
case AutoSmoothing: {
// For the AutoSmooth case, don't do anything! Keep the default settings.
break;
}
default:
ASSERT_NOT_REACHED();
}
uint32_t oldFontSmoothingStyle = wkSetFontSmoothingStyle(cgContext, shouldUseFontSmoothing);
const FontPlatformData& platformData = font->platformData();
CGContextSetFont(cgContext, platformData.cgFont());
CGAffineTransform matrix = CGAffineTransformIdentity;
matrix.b = -matrix.b;
matrix.d = -matrix.d;
if (platformData.syntheticOblique()) {
static float skew = -tanf(syntheticObliqueAngle * piFloat / 180.0f);
matrix = CGAffineTransformConcat(matrix, CGAffineTransformMake(1, 0, skew, 1, 0, 0));
}
CGContextSetTextMatrix(cgContext, matrix);
// Uniscribe gives us offsets to help refine the positioning of combining glyphs.
FloatSize translation = glyphBuffer.offsetAt(from);
CGContextSetFontSize(cgContext, platformData.size());
wkSetCGContextFontRenderingStyle(cgContext, font->isSystemFont(), false, font->platformData().useGDI());
FloatSize shadowOffset;
float shadowBlur;
Color shadowColor;
ColorSpace shadowColorSpace;
graphicsContext->getShadow(shadowOffset, shadowBlur, shadowColor, shadowColorSpace);
bool hasSimpleShadow = graphicsContext->textDrawingMode() == TextModeFill && shadowColor.isValid() && !shadowBlur && (!graphicsContext->shadowsIgnoreTransforms() || graphicsContext->getCTM().isIdentityOrTranslationOrFlipped());
if (hasSimpleShadow) {
// Paint simple shadows ourselves instead of relying on CG shadows, to avoid losing subpixel antialiasing.
graphicsContext->clearShadow();
Color fillColor = graphicsContext->fillColor();
ColorSpace fillColorSpace = graphicsContext->fillColorSpace();
Color shadowFillColor(shadowColor.red(), shadowColor.green(), shadowColor.blue(), shadowColor.alpha() * fillColor.alpha() / 255);
graphicsContext->setFillColor(shadowFillColor, shadowColorSpace);
float shadowTextX = point.x() + translation.width() + shadowOffset.width();
// If shadows are ignoring transforms, then we haven't applied the Y coordinate flip yet, so down is negative.
float shadowTextY = point.y() + translation.height() + shadowOffset.height() * (graphicsContext->shadowsIgnoreTransforms() ? -1 : 1);
CGContextSetTextPosition(cgContext, shadowTextX, shadowTextY);
CGContextShowGlyphsWithAdvances(cgContext, glyphBuffer.glyphs(from), static_cast<const CGSize*>(glyphBuffer.advances(from)), numGlyphs);
if (font->syntheticBoldOffset()) {
CGContextSetTextPosition(cgContext, point.x() + translation.width() + shadowOffset.width() + font->syntheticBoldOffset(), point.y() + translation.height() + shadowOffset.height());
CGContextShowGlyphsWithAdvances(cgContext, glyphBuffer.glyphs(from), static_cast<const CGSize*>(glyphBuffer.advances(from)), numGlyphs);
}
graphicsContext->setFillColor(fillColor, fillColorSpace);
}
CGContextSetTextPosition(cgContext, point.x() + translation.width(), point.y() + translation.height());
CGContextShowGlyphsWithAdvances(cgContext, glyphBuffer.glyphs(from), static_cast<const CGSize*>(glyphBuffer.advances(from)), numGlyphs);
if (font->syntheticBoldOffset()) {
CGContextSetTextPosition(cgContext, point.x() + translation.width() + font->syntheticBoldOffset(), point.y() + translation.height());
CGContextShowGlyphsWithAdvances(cgContext, glyphBuffer.glyphs(from), static_cast<const CGSize*>(glyphBuffer.advances(from)), numGlyphs);
}
if (hasSimpleShadow)
graphicsContext->setShadow(shadowOffset, shadowBlur, shadowColor, shadowColorSpace);
wkRestoreFontSmoothingStyle(cgContext, oldFontSmoothingStyle);
}
void init()
{
setupGL();
Surface::init();
cam = new Camera();
mdl = new Model();
Mesh mesh = loadMeshFromObj("resources/meshes/dragon.obj", 0.1);
Geometry dragon = createGeometryFromMesh(mesh);
Surface *surface = new Surface();
//surface->loadDiffuseTexture("resources/meshes/textures/sponza_floor_a_spec.tga");
surfaces.insert(std::pair<std::string, Surface*> (std::string("default"), surface) );
mdl->addGeometryAndSurface(&dragon, surface);
// Geometry floor;
// Geometry::sVertex v;
// v.position = vec3(-1, 0,-1); v.texCoord = vec2(0,0); floor.addVertex(v);
// v.position = vec3( 1, 0,-1); v.texCoord = vec2(1,0); floor.addVertex(v);
// v.position = vec3( 1, 0, 1); v.texCoord = vec2(1,1); floor.addVertex(v);
// v.position = vec3(-1, 0, 1); v.texCoord = vec2(0,1); floor.addVertex(v);
// floor.addTriangle(uvec3(0,2,1));
// floor.addTriangle(uvec3(0,3,2));
// mdl->addGeometryAndSurface(&floor, surface);
model = new Geometry();
mesh = loadMeshFromObj("resources/meshes/sponza.obj", 0.01f);
*model = createGeometryFromMesh(mesh);
model->createStaticBuffers();
std::vector<Mesh> meshes = loadMeshesFromObj("resources/meshes/sponza.obj", 0.01f);
std::vector<Geometry> geometries = createGeometryFromMesh(meshes);
std::vector<Material> materials = loadMaterialsFromMtl("resources/meshes/sponza.mtl");
surfaces = createSurfaceFromMaterial(materials, "resources/meshes/");
for(unsigned int i=0; i<geometries.size(); ++i)
{
mdl->addGeometryAndSurface(&geometries[i], surfaces[geometries[i].material]);
}
mdl->prepare();
fsquad = new Geometry();
Geometry::sVertex v;
v.position = vec3(-1,-1, 0); v.texCoord = vec2(0,0); fsquad->addVertex(v);
v.position = vec3( 1,-1, 0); v.texCoord = vec2(1,0); fsquad->addVertex(v);
v.position = vec3( 1, 1, 0); v.texCoord = vec2(1,1); fsquad->addVertex(v);
v.position = vec3(-1, 1, 0); v.texCoord = vec2(0,1); fsquad->addVertex(v);
fsquad->addTriangle(uvec3(0,1,2));
fsquad->addTriangle(uvec3(0,2,3));
fsquad->createStaticBuffers();
geometryShader = new Shader("resources/shaders/geometry_vert.glsl",
"resources/shaders/geometry_frag.glsl");
geometryBackShader = new Shader("resources/shaders/geometry_vert.glsl",
"resources/shaders/geometry_back_frag.glsl");
hbaoHalfShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/hbao_frag.glsl");
hbaoFullShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/hbao_full_frag.glsl");
compositShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/composit_frag.glsl");
blurXShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/blur_x_frag.glsl");
blurYShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/blur_y_frag.glsl");
downsampleShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/downsample_depth_frag.glsl");
upsampleShader = new Shader("resources/shaders/fullscreen_vert.glsl",
"resources/shaders/upsample_aoz_frag.glsl");
// Full res deferred base
fboFullRes = new Framebuffer2D(WIDTH, HEIGHT);
fboFullRes->attachBuffer(FBO_DEPTH, GL_DEPTH_COMPONENT32, GL_DEPTH_COMPONENT, GL_FLOAT, GL_LINEAR, GL_LINEAR);
fboFullRes->attachBuffer(FBO_AUX0, GL_RGBA8, GL_RGBA, GL_FLOAT);
fboFullRes->attachBuffer(FBO_AUX1, GL_RG16F, GL_RG, GL_FLOAT, GL_LINEAR, GL_LINEAR);
fboFullRes->attachBuffer(FBO_AUX2, GL_RG16F, GL_RG, GL_FLOAT, GL_LINEAR, GL_LINEAR);
//fboFullRes->attachBuffer(FBO_AUX3, GL_R8, GL_RED, GL_FLOAT);
// Half res buffer for AO
fboHalfRes = new Framebuffer2D(AO_WIDTH, AO_HEIGHT);
//fboHalfRes->attachBuffer(FBO_DEPTH, GL_DEPTH_COMPONENT32, GL_DEPTH_COMPONENT, GL_FLOAT);
fboHalfRes->attachBuffer(FBO_AUX0, GL_R32F, GL_RED, GL_FLOAT, GL_LINEAR, GL_LINEAR);
fboHalfRes->attachBuffer(FBO_AUX1, GL_R8, GL_RED, GL_FLOAT, GL_LINEAR, GL_LINEAR);
float fovRad = cam->getFov() * 3.14159265f / 180.0f;
vec2 FocalLen, InvFocalLen, UVToViewA, UVToViewB, LinMAD;
FocalLen[0] = 1.0f / tanf(fovRad * 0.5f) * ((float)AO_HEIGHT / (float)AO_WIDTH);
FocalLen[1] = 1.0f / tanf(fovRad * 0.5f);
InvFocalLen[0] = 1.0f / FocalLen[0];
InvFocalLen[1] = 1.0f / FocalLen[1];
UVToViewA[0] = -2.0f * InvFocalLen[0];
UVToViewA[1] = -2.0f * InvFocalLen[1];
UVToViewB[0] = 1.0f * InvFocalLen[0];
UVToViewB[1] = 1.0f * InvFocalLen[1];
float near = cam->getNear(), far = cam->getFar();
LinMAD[0] = (near-far)/(2.0f*near*far);
LinMAD[1] = (near+far)/(2.0f*near*far);
hbaoHalfShader->bind();
int pos;
pos = hbaoHalfShader->getUniformLocation("FocalLen");
glUniform2f(pos, FocalLen[0], FocalLen[1]);
pos = hbaoHalfShader->getUniformLocation("UVToViewA");
glUniform2f(pos, UVToViewA[0], UVToViewA[1]);
pos = hbaoHalfShader->getUniformLocation("UVToViewB");
glUniform2f(pos, UVToViewB[0], UVToViewB[1]);
pos = hbaoHalfShader->getUniformLocation("LinMAD");
glUniform2f(pos, LinMAD[0], LinMAD[1]);
pos = hbaoHalfShader->getUniformLocation("AORes");
glUniform2f(pos, (float)AO_WIDTH, (float)AO_HEIGHT);
pos = hbaoHalfShader->getUniformLocation("InvAORes");
glUniform2f(pos, 1.0f/(float)AO_WIDTH, 1.0f/(float)AO_HEIGHT);
pos = hbaoHalfShader->getUniformLocation("R");
glUniform1f(pos, AO_RADIUS);
pos = hbaoHalfShader->getUniformLocation("R2");
glUniform1f(pos, AO_RADIUS*AO_RADIUS);
pos = hbaoHalfShader->getUniformLocation("NegInvR2");
glUniform1f(pos, -1.0f / (AO_RADIUS*AO_RADIUS));
pos = hbaoHalfShader->getUniformLocation("MaxRadiusPixels");
glUniform1f(pos, AO_MAX_RADIUS_PIXELS / (float)RES_RATIO);
pos = hbaoHalfShader->getUniformLocation("NoiseScale");
glUniform2f(pos, (float)AO_WIDTH/(float)NOISE_RES, (float)AO_HEIGHT/(float)NOISE_RES);
pos = hbaoHalfShader->getUniformLocation("NumDirections");
glUniform1i(pos, AO_DIRS);
pos = hbaoHalfShader->getUniformLocation("NumSamples");
glUniform1i(pos, AO_SAMPLES);
hbaoFullShader->bind();
pos = hbaoFullShader->getUniformLocation("FocalLen");
glUniform2f(pos, FocalLen[0], FocalLen[1]);
pos = hbaoFullShader->getUniformLocation("UVToViewA");
glUniform2f(pos, UVToViewA[0], UVToViewA[1]);
pos = hbaoFullShader->getUniformLocation("UVToViewB");
glUniform2f(pos, UVToViewB[0], UVToViewB[1]);
pos = hbaoFullShader->getUniformLocation("LinMAD");
glUniform2f(pos, LinMAD[0], LinMAD[1]);
pos = hbaoFullShader->getUniformLocation("AORes");
glUniform2f(pos, (float)WIDTH, (float)HEIGHT);
pos = hbaoFullShader->getUniformLocation("InvAORes");
glUniform2f(pos, 1.0f/(float)WIDTH, 1.0f/(float)HEIGHT);
pos = hbaoFullShader->getUniformLocation("R");
glUniform1f(pos, AO_RADIUS);
pos = hbaoFullShader->getUniformLocation("R2");
glUniform1f(pos, AO_RADIUS*AO_RADIUS);
pos = hbaoFullShader->getUniformLocation("NegInvR2");
glUniform1f(pos, -1.0f / (AO_RADIUS*AO_RADIUS));
pos = hbaoFullShader->getUniformLocation("MaxRadiusPixels");
glUniform1f(pos, AO_MAX_RADIUS_PIXELS);
pos = hbaoFullShader->getUniformLocation("NoiseScale");
glUniform2f(pos, (float)WIDTH/(float)NOISE_RES, (float)HEIGHT/(float)NOISE_RES);
pos = hbaoFullShader->getUniformLocation("NumDirections");
glUniform1i(pos, AO_DIRS);
pos = hbaoFullShader->getUniformLocation("NumSamples");
glUniform1i(pos, AO_SAMPLES);
blurXShader->bind();
pos = blurXShader->getUniformLocation("AORes");
glUniform2f(pos, AO_WIDTH, AO_HEIGHT);
pos = blurXShader->getUniformLocation("InvAORes");
glUniform2f(pos, 1.0f/AO_WIDTH, 1.0f/AO_HEIGHT);
pos = blurXShader->getUniformLocation("FullRes");
glUniform2f(pos, WIDTH, HEIGHT);
pos = blurXShader->getUniformLocation("InvFullRes");
glUniform2f(pos, 1.0f/WIDTH, 1.0f/HEIGHT);
pos = blurXShader->getUniformLocation("LinMAD");
glUniform2f(pos, LinMAD[0], LinMAD[1]);
blurYShader->bind();
pos = blurYShader->getUniformLocation("AORes");
glUniform2f(pos, AO_WIDTH, AO_HEIGHT);
pos = blurYShader->getUniformLocation("InvAORes");
glUniform2f(pos, 1.0f/AO_WIDTH, 1.0f/AO_HEIGHT);
pos = blurYShader->getUniformLocation("FullRes");
glUniform2f(pos, WIDTH, HEIGHT);
pos = blurYShader->getUniformLocation("InvFullRes");
glUniform2f(pos, 1.0f/WIDTH, 1.0f/HEIGHT);
pos = blurYShader->getUniformLocation("LinMAD");
glUniform2f(pos, LinMAD[0], LinMAD[1]);
downsampleShader->bind();
pos = downsampleShader->getUniformLocation("LinMAD");
glUniform2f(pos, LinMAD[0], LinMAD[1]);
pos = downsampleShader->getUniformLocation("ResRatio");
glUniform1i(pos, RES_RATIO);
upsampleShader->bind();
pos = upsampleShader->getUniformLocation("LinMAD");
glUniform2f(pos, LinMAD[0], LinMAD[1]);
glGenTextures(1, &noiseTexture);
glBindTexture(GL_TEXTURE_2D, noiseTexture);
generateNoiseTexture(NOISE_RES, NOISE_RES);
}
#include "ofxVirtualCamera.h"
const float fovWidth = 1.0144686707507438;
const float fovHeight = 0.78980943449644714;
const float XtoZ = tanf(fovWidth/2)*2;
const float YtoZ = tanf(fovHeight/2)*2;
const unsigned int camWidth = 640;
const unsigned int camHeight = 480;
ofxVec3f ConvertProjectiveToRealWorld(float x, float y, float z) {
return ofxVec3f(-(x/camWidth-.5f) * z * XtoZ, // -x for front-mounted cameras
(y/camHeight-.5f) * z * YtoZ,
z);
}
ofxVirtualCamera::ofxVirtualCamera() :
newFrame(false),
maxLen(1),
stepSize(1),
nearClipping(-1024),
farClipping(1024),
orthoScale(1),
position(ofxVec3f(0, 0, 0)),
rotation(ofxVec3f(0, 0, 0)) {
}
ofxVirtualCamera::~ofxVirtualCamera() {
kinect.close();
}
void ofxVirtualCamera::setup() {
// Xforms
void light::xform_calc ()
{
if (Device.dwFrame == m_xform_frame) return;
m_xform_frame = Device.dwFrame;
// build final rotation / translation
Fvector L_dir,L_up,L_right;
// dir
L_dir.set (direction);
float l_dir_m = L_dir.magnitude();
if (_valid(l_dir_m) && l_dir_m>EPS_S) L_dir.div(l_dir_m);
else L_dir.set(0,0,1);
// R&N
if (right.square_magnitude()>EPS) {
// use specified 'up' and 'right', just enshure ortho-normalization
L_right.set (right); L_right.normalize ();
L_up.crossproduct (L_dir,L_right); L_up.normalize ();
L_right.crossproduct (L_up,L_dir); L_right.normalize ();
} else {
// auto find 'up' and 'right' vectors
L_up.set (0,1,0); if (_abs(L_up.dotproduct(L_dir))>.99f) L_up.set(0,0,1);
L_right.crossproduct (L_up,L_dir); L_right.normalize ();
L_up.crossproduct (L_dir,L_right); L_up.normalize ();
}
// matrix
Fmatrix mR;
mR.i = L_right; mR._14 = 0;
mR.j = L_up; mR._24 = 0;
mR.k = L_dir; mR._34 = 0;
mR.c = position; mR._44 = 1;
// switch
switch(flags.type) {
case IRender_Light::REFLECTED :
case IRender_Light::POINT :
{
// scale of identity sphere
float L_R = range;
Fmatrix mScale; mScale.scale (L_R,L_R,L_R);
m_xform.mul_43 (mR,mScale);
}
break;
case IRender_Light::SPOT :
{
// scale to account range and angle
float s = 2.f*range*tanf(cone/2.f);
Fmatrix mScale; mScale.scale(s,s,range); // make range and radius
m_xform.mul_43 (mR,mScale);
}
break;
case IRender_Light::OMNIPART :
{
float L_R = 2*range; // volume is half-radius
Fmatrix mScale; mScale.scale (L_R,L_R,L_R);
m_xform.mul_43 (mR,mScale);
}
break;
default:
m_xform.identity ();
break;
}
}
int main(int argc, char** argv)
{
GLFWwindow window;
int ch, iter;
double dt;
double last_update_time;
int frame;
float f;
GLint uloc_modelview;
GLint uloc_project;
char* vertex_shader_path = NULL;
char* fragment_shader_path = NULL;
char* vertex_shader_src = NULL;
char* fragment_shader_src = NULL;
GLuint shader_program;
while ((ch = getopt(argc, argv, "f:v:h")) != -1)
{
switch (ch)
{
case 'f':
fragment_shader_path = optarg;
break;
case 'v':
vertex_shader_path = optarg;
break;
case 'h':
usage();
exit(EXIT_SUCCESS);
default:
usage();
exit(EXIT_FAILURE);
}
}
if (fragment_shader_path)
{
vertex_shader_src = read_file_content(fragment_shader_path);
if (!fragment_shader_src)
{
fprintf(stderr,
"ERROR: unable to load fragment shader from '%s'\n",
fragment_shader_path);
exit(EXIT_FAILURE);
}
}
if (vertex_shader_path)
{
vertex_shader_src = read_file_content(vertex_shader_path);
if (!vertex_shader_src)
{
fprintf(stderr,
"ERROR: unable to load vertex shader from '%s'\n",
fragment_shader_path);
exit(EXIT_FAILURE);
}
}
if (!glfwInit())
{
fprintf(stderr, "ERROR: Unable to initialize GLFW\n");
usage();
free(vertex_shader_src);
free(fragment_shader_src);
exit(EXIT_FAILURE);
}
glfwWindowHint(GLFW_WINDOW_RESIZABLE, GL_FALSE);
glfwWindowHint(GLFW_OPENGL_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_OPENGL_VERSION_MINOR, 2);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_FALSE);
window = glfwCreateWindow(800, 600, GLFW_WINDOWED, "GLFW OpenGL3 Heightmap demo", NULL);
if (! window )
{
fprintf(stderr, "ERROR: Unable to create the OpenGL context and associated window\n");
usage();
free(vertex_shader_src);
free(fragment_shader_src);
exit(EXIT_FAILURE);
}
/* Register events callback */
glfwSetWindowCloseCallback(window_close_callback);
glfwSetKeyCallback(key_callback);
glfwMakeContextCurrent(window);
if (GL_TRUE != init_opengl())
{
fprintf(stderr, "ERROR: unable to resolve OpenGL function pointers\n");
free(vertex_shader_src);
free(fragment_shader_src);
exit(EXIT_FAILURE);
}
/* Prepare opengl resources for rendering */
shader_program = make_shader_program(vertex_shader_src , fragment_shader_src);
free(vertex_shader_src);
free(fragment_shader_src);
if (shader_program == 0u)
{
fprintf(stderr, "ERROR: during creation of the shader program\n");
usage();
exit(EXIT_FAILURE);
}
pglUseProgram(shader_program);
uloc_project = pglGetUniformLocation(shader_program, "project");
uloc_modelview = pglGetUniformLocation(shader_program, "modelview");
/* Compute the projection matrix */
f = 1.0f / tanf(view_angle / 2.0f);
projection_matrix[0] = f / aspect_ratio;
projection_matrix[5] = f;
projection_matrix[10] = (z_far + z_near)/ (z_near - z_far);
projection_matrix[11] = -1.0f;
projection_matrix[14] = 2.0f * (z_far * z_near) / (z_near - z_far);
pglUniformMatrix4fv(uloc_project, 1, GL_FALSE, projection_matrix);
/* Set the camera position */
modelview_matrix[12] = -5.0f;
modelview_matrix[13] = -5.0f;
modelview_matrix[14] = -20.0f;
pglUniformMatrix4fv(uloc_modelview, 1, GL_FALSE, modelview_matrix);
/* Create mesh data */
init_map();
make_mesh(shader_program);
/* Create vao + vbo to store the mesh */
/* Create the vbo to store all the information for the grid and the height */
/* setup the scene ready for rendering */
glViewport(0, 0, 800, 600);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
/* main loop */
frame = 0;
iter = 0;
dt = last_update_time = glfwGetTime();
while (running)
{
++frame;
/* render the next frame */
glClear(GL_COLOR_BUFFER_BIT);
glDrawElements(GL_LINES, 2* MAP_NUM_LINES , GL_UNSIGNED_INT, 0);
/* display and process events through callbacks */
glfwSwapBuffers(window);
glfwPollEvents();
/* Check the frame rate and update the heightmap if needed */
dt = glfwGetTime();
if ((dt - last_update_time) > 0.2)
{
/* generate the next iteration of the heightmap */
if (iter < MAX_ITER)
{
update_map(NUM_ITER_AT_A_TIME);
update_mesh();
iter += NUM_ITER_AT_A_TIME;
}
last_update_time = dt;
frame = 0;
}
}
exit(EXIT_SUCCESS);
}
void
MulticopterPositionControl::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
/*
* do subscriptions
*/
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
parameters_update(true);
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/* get an initial update for all sensor and status data */
poll_subscriptions();
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool reset_yaw_sp = true;
bool was_armed = false;
hrt_abstime t_prev = 0;
_hover_time = 0.0; // miao:
_mode_mission = 1;
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
/* wakeup source */
struct pollfd fds[1];
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
poll_subscriptions();
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
t_prev = t;
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
reset_yaw_sp = true;
_reset_mission = true;//miao:
}
//Update previous arming state
was_armed = _control_mode.flag_armed;
update_ref();
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
_vel_ff.zero();
_sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;
} else if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
_mode_auto = false;
} else {
/* AUTO */
control_auto(dt);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
//if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// miao: for auto landing test with manual mode
if (_mode_mission==3) {
_vel_sp(2) = _params.land_speed;
}
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual.z;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max_air;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
float thrust_abs = thrust_sp.length();
if (thrust_abs > _params.thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > _params.thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -_params.thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = _params.thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = _params.thr_max;
}
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
/* save thrust setpoint for logging */
_local_pos_sp.acc_x = thrust_sp(0);
_local_pos_sp.acc_x = thrust_sp(1);
_local_pos_sp.acc_x = thrust_sp(2);
_att_sp.timestamp = hrt_absolute_time();
} else {
reset_int_z = true;
}
}
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
/* publish local position setpoint */
if (_local_pos_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
}
// generate attitude setpoint from manual controls
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
// reset yaw setpoint to current position if needed
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
}
// do not move yaw while arming
else if (_manual.z > 0.1f)
{
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
}
}
//Control roll and pitch directly if we no aiding velocity controller is active
if(!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//Control climb rate directly if no aiding altitude controller is active
if(!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = _manual.z;
}
//Construct attitude setpoint rotation matrix
math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
_att_sp.timestamp = hrt_absolute_time();
}
else {
reset_yaw_sp = true;
}
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
}
warnx("stopped");
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
_control_task = -1;
_exit(0);
}
//----------------------------------------
float ofCamera::getImagePlaneDistance(ofRectangle viewport) const {
return viewport.height / (2.0f * tanf(PI * fov / 360.0f));
}
void ff_lowpass_init(LPFilterContext *s, float sample_rate, float fpass, float fstop, float apass, float astop){
int i, j;
float fp = sample_rate*tanf(M_PI*fpass/sample_rate)/M_PI;
float fs = sample_rate*tanf(M_PI*fstop/sample_rate)/M_PI;
float ws = fs/fp;
float vp = 2*M_PI*fp;
int N = (int)ceilf(log10f((pow(10, astop/10)-1) / (pow(10, apass/10)-1))/(2*log10f(ws)));
float w0 = ws/pow(pow(10, astop/10)-1, 1.0/(2.0*N));
float dfi0 = M_PI/N;
complex *p, *pt;
complex gain = 1.0;
p = av_malloc(N*sizeof(*p));
pt = av_malloc((N+1)*sizeof(*pt));
for(i=0; i<2; i++){
s->filterCoeffs[i] = av_malloc((N+1)*sizeof(*s->filterCoeffs[i]));
s->buf[i] = av_malloc((N+1)*sizeof(*s->buf[i]));
}
for(i=0; i<N; i++){
s->buf[0][i] = s->buf[1][i] = 0;
}
av_log(NULL, AV_LOG_DEBUG, "fp=%f fs=%f\n", fp, fs);
av_log(NULL, AV_LOG_DEBUG, "vp=%f\n", vp);
av_log(NULL, AV_LOG_DEBUG, "ws=%f\n", ws);
av_log(NULL, AV_LOG_DEBUG, "N=%i w0=%f\n", N, w0);
for(i=0; i<N; i++){
p[i] = w0 * cexp(I*(M_PI/2.0 + (i+0.5)*dfi0));
gain *= -p[i];
p[i] *= vp;
gain *= vp/(2.0*sample_rate-p[i]);
p[i] = (2.0*sample_rate+p[i])/(2.0*sample_rate-p[i]);
av_log(NULL, AV_LOG_DEBUG, "p[%i]=%f+%fI\n", i, creal(p[i]), cimag(p[i]));
}
av_log(NULL, AV_LOG_DEBUG, "gain=%f+%fI\n", creal(gain), cimag(gain));
for(i=0; i<N; i++){
pt[i] = 1;
for(j=i; j>0; j--)
pt[j] = -pt[j]*p[i] + pt[j-1];
pt[0] *= -p[i];
}
for(i=0; i<=N; i++){
av_log(NULL, AV_LOG_DEBUG, "a %i: %f\n", i, creal(pt[i]));
}
pt[N]=1;
for(i=0; i<=N/2; i++){
complex t;
t=pt[i];
pt[i] = pt[N-i];
pt[N-i] = t;
}
for(i=0; i<=N; i++){
av_log(NULL, AV_LOG_DEBUG, "%i: %f\n", i, creal(pt[i]));
}
for(i=0; i<N; i++)
s->filterCoeffs[0][i] = creal(pt[i+1]);
s->filterCoeffs[0][N] = 0;
av_free(p);
av_free(pt);
for(i=0; i<N; i++){
s->filterCoeffs[1][i] = gain;
for(j=i; j>0; j--)
s->filterCoeffs[1][j] = s->filterCoeffs[1][j] + s->filterCoeffs[1][j-1];
}
s->filterCoeffs[1][N] = gain;
for(i=0; i<=N; i++){
av_log(NULL, AV_LOG_DEBUG, "%i: ac=%f bc=%f\n", i, s->filterCoeffs[0][i], s->filterCoeffs[1][i]);
}
s->N = N;
}
//函数名：CtrlAlti()
//输入：无
//输出: 最终结果输出到全局变量thrustZSp
//描述：控制高度，也就是高度悬停控制函数
//only in climb rate mode and landind mode. now we don't work on manual mode
void CtrlAlti(void)
{
float manThr=0,alt=0,velZ=0;
float altSp=0;
float posZVelSp=0;
float altSpOffset,altSpOffsetMax=0;
float dt=0,t=0;
static float tPrev=0,velZPrev=0;
float posZErr=0,velZErr=0,valZErrD=0;
float thrustXYSpLen=0,thrustSpLen=0;
float thrustXYMax=0;
//get dt
//保证dt运算不能被打断，保持更新，否则dt过大，积分爆满。
if(tPrev==0){
tPrev=micros();
return;
}else{
t=micros();
dt=(t-tPrev) /1000000.0f;
tPrev=t;
}
//only in climb rate mode and landind mode. now we don't work on manual mode
//手动模式不使用该高度控制算法
if(MANUAL == altCtrlMode || !FLY_ENABLE){
return;
}
//--------------pos z ctrol---------------//
//get current alt
alt=-nav.z;
//get desired move rate from stick
manThr=RC_DATA.THROTTLE / 1000.0f;
spZMoveRate= -dbScaleLinear(manThr-0.5f,0.5f,ALT_CTRL_Z_DB); // scale to -1~1 . NED frame
spZMoveRate = spZMoveRate * ALT_VEL_MAX; // scale to vel min max
//get alt setpoint in CLIMB rate mode
altSp =-nav.z; //only alt is not in ned frame.
altSp -= spZMoveRate * dt;
//limit alt setpoint
altSpOffsetMax=ALT_VEL_MAX / alt_PID.P * 2.0f;
altSpOffset = altSp-alt;
if( altSpOffset > altSpOffsetMax){
altSp=alt + altSpOffsetMax;
}else if( altSpOffset < -altSpOffsetMax){
altSp=alt - altSpOffsetMax;
}
//限高
if(isAltLimit)
{
if(altSp - altLand > ALT_LIMIT)
{
altSp=altLand+ALT_LIMIT;
spZMoveRate=0;
}
}
// pid and feedforward control . in ned frame
posZErr= -(altSp - alt);
posZVelSp = posZErr * alt_PID.P + spZMoveRate * ALT_FEED_FORWARD;
//consider landing mode
if(altCtrlMode==LANDING)
posZVelSp = LAND_SPEED;
//获取一个预估的Z轴悬停基准值，相关因素有电池电压
//get hold throttle. give it a estimated value
if(zIntReset){
thrustZInt = estimateHoverThru();
zIntReset = 0;
}
velZ=nav.vz;
velZErr = posZVelSp - velZ;
valZErrD = (spZMoveRate - velZ) * alt_PID.P - (velZ - velZPrev) / dt; //spZMoveRate is from manual stick vel control
velZPrev=velZ;
thrustZSp= velZErr * alt_vel_PID.P + valZErrD * alt_vel_PID.D + thrustZInt; //in ned frame. thrustZInt contains hover thrust
//limit thrust min !!
if(altCtrlMode!=LANDING){
if (-thrustZSp < THR_MIN){
thrustZSp = -THR_MIN;
}
}
//与动力分配相关 testing
satXY=0;
satZ=0;
thrustXYSp[0]= sinf(RC_DATA.ROOL * M_PI_F /180.0f) ;//目标角度转加速度
thrustXYSp[1]= sinf(RC_DATA.PITCH * M_PI_F /180.0f) ; //归一化
thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);
//limit tilt max
if(thrustXYSpLen >0.01f )
{
thrustXYMax=-thrustZSp * tanf(TILT_MAX);
if(thrustXYSpLen > thrustXYMax)
{
float k=thrustXYMax / thrustXYSpLen;
thrustXYSp[1] *= k;
thrustXYSp[0] *= k;
satXY=1;
thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);
}
}
//limit max thrust!!
thrustSpLen=sqrtf(thrustXYSpLen * thrustXYSpLen + thrustZSp * thrustZSp);
if(thrustSpLen > THR_MAX)
{
if(thrustZSp < 0.0f) //going up
{
if (-thrustZSp > THR_MAX)
{
/* thrust Z component is too large, limit it */
thrustXYSp[0] = 0.0f;
thrustXYSp[1] = 0.0f;
thrustZSp = -THR_MAX;
satXY = 1;
satZ = 1;
}
else {
float k = 0;
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
thrustXYMax = sqrtf(THR_MAX * THR_MAX- thrustZSp * thrustZSp);
k=thrustXYMax / thrustXYSpLen;
thrustXYSp[1] *=k;
thrustXYSp[0] *= k;
satXY=1;
}
}
else { //going down
/* Z component is negative, going down, simply limit thrust vector */
float k = THR_MAX / thrustSpLen;
thrustZSp *= k;
thrustXYSp[1] *=k;
thrustXYSp[0] *= k;
satXY = 1;
satZ = 1;
}
}
rollSp= asinf(thrustXYSp[0]) * 180.0f /M_PI_F;
pitchSp = asinf(thrustXYSp[1]) * 180.0f /M_PI_F;
// if saturation ,don't integral
if(!satZ )//&& fabs(thrustZSp)<THR_MAX
{
thrustZInt += velZErr * alt_vel_PID.I * dt;
if (thrustZInt > 0.0f)
thrustZInt = 0.0f;
}
}
void gameRender()
{
// update simulation
float dt = g_timer.getElapsedMs() * 0.001f;
g_timer.reset();
// filter motion so you slow down gradually when you lift your finger off the screen
const float kChangeFactor = 0.1f;
g_playerPosChange = g_playerPosChangeTarget * kChangeFactor + g_playerPosChange * (1.0f - kChangeFactor);
g_playerAngleChange = g_playerAngleChangeTarget * kChangeFactor + g_playerAngleChange * (1.0f - kChangeFactor);
// update engine sound
g_engineSound->setPaused(g_playerPosChange == 0.0f);
float normSpeed = fabs(g_playerPosChange / kPosChangeMax);
g_engineSound->setPitchShift(normSpeed*8);
g_engineSound->setVolume(normSpeed * 0.4f);
// update player position
Vector3 forwardDir(sinf(g_playerAngle), 0.0f, cosf(g_playerAngle));
Vector3 dr;
Vector3::multiply(forwardDir, g_playerPosChange*dt, dr);
Vector3::add(g_playerPos, dr, g_playerPos);
g_playerAngle += g_playerAngleChange * dt;
// keep player angle in [0..2pi]
if (g_playerAngle > 2.0f*M_PI)
{
g_playerAngle -= 2.0f*M_PI;
}
else if (g_playerAngle < 0.0f)
{
g_playerAngle += 2.0f*M_PI;
}
// test for hits
for (int i = 0; i < kNumCubes; ++i)
{
if (g_cubes[i]->hitTest(g_playerPos))
{
g_pointSound->play();
placeCube(g_cubes[i]);
}
}
// render
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
float fov = 0.9f;
float near = 0.1f;
float far = 100.0f;
float aspect = (float) g_width / g_height;
float top = tanf(fov*0.5) * near;
float bottom = -top;
float left = aspect * bottom;
float right = aspect * top;
glFrustumf(left, right, bottom, top, near, far);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// for landscape orientation
glRotatef(90.0f, 0.0f, 0.0f, 1.0f);
Vector3 eye;
Vector3 v;
Vector3::multiply(forwardDir, -3.0f, v);
Vector3::add(g_playerPos, v, eye);
eye.y = 1.0f;
Vector3::multiply(forwardDir, 1.0f, v);
Vector3 center;
Vector3::add(g_playerPos, v, center);
gluLookAt(eye.x, eye.y, eye.z,
center.x, center.y, center.z,
0.0f, 1.0f, 0.0f);
CkSound::set3dListenerPosition(
eye.x, eye.y, eye.z,
center.x, center.y, center.z,
0.0f, 1.0f, 0.0f);
g_ship->setTransform(g_playerPos, g_playerAngle * 180.0f / M_PI + 180.0f);
g_ship->draw();
for (int i = 0; i < kNumCubes; ++i)
{
g_cubes[i]->draw(dt);
}
g_ground->draw();
CkMixer::getMaster()->setVolume(1.0f);
CkUpdate();
}
void GuiViewport3D::onDraw(sf::RenderTarget& window)
{
#if FEATURE_3D_RENDERING
if (my_spaceship)
soundManager->setListenerPosition(my_spaceship->getPosition(), my_spaceship->getRotation());
else
soundManager->setListenerPosition(sf::Vector2f(camera_position.x, camera_position.y), camera_yaw);
window.popGLStates();
ShaderManager::getShader("billboardShader")->setUniform("camera_position", camera_position);
float camera_fov = 60.0f;
float sx = window.getSize().x * window.getView().getViewport().width / window.getView().getSize().x;
float sy = window.getSize().y * window.getView().getViewport().height / window.getView().getSize().y;
glViewport(rect.left * sx, (float(window.getView().getSize().y) - rect.height - rect.top) * sx, rect.width * sx, rect.height * sy);
glClearDepth(1.f);
glClear(GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
glDepthMask(GL_TRUE);
glEnable(GL_CULL_FACE);
glColor4f(1,1,1,1);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
_glPerspective(camera_fov, rect.width/rect.height, 1.f, 25000.f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glRotatef(90, 1, 0, 0);
glScalef(1,1,-1);
glRotatef(-camera_pitch, 1, 0, 0);
glRotatef(-camera_yaw - 90, 0, 0, 1);
glGetDoublev(GL_PROJECTION_MATRIX, projection_matrix);
glGetDoublev(GL_MODELVIEW_MATRIX, model_matrix);
glGetDoublev(GL_VIEWPORT, viewport);
glDepthMask(false);
sf::Texture::bind(textureManager.getTexture("StarsBack"), sf::Texture::Normalized);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(0.0, 0.0); glVertex3f( 100, 100, 100);
glTexCoord2f(0.0, 1.0); glVertex3f( 100, 100,-100);
glTexCoord2f(1.0, 0.0); glVertex3f(-100, 100, 100);
glTexCoord2f(1.0, 1.0); glVertex3f(-100, 100,-100);
glEnd();
sf::Texture::bind(textureManager.getTexture("StarsLeft"), sf::Texture::Normalized);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(0.0, 0.0); glVertex3f(-100, 100, 100);
glTexCoord2f(0.0, 1.0); glVertex3f(-100, 100,-100);
glTexCoord2f(1.0, 0.0); glVertex3f(-100,-100, 100);
glTexCoord2f(1.0, 1.0); glVertex3f(-100,-100,-100);
glEnd();
sf::Texture::bind(textureManager.getTexture("StarsFront"), sf::Texture::Normalized);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(0.0, 0.0); glVertex3f(-100,-100, 100);
glTexCoord2f(0.0, 1.0); glVertex3f(-100,-100,-100);
glTexCoord2f(1.0, 0.0); glVertex3f( 100,-100, 100);
glTexCoord2f(1.0, 1.0); glVertex3f( 100,-100,-100);
glEnd();
sf::Texture::bind(textureManager.getTexture("StarsRight"), sf::Texture::Normalized);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(0.0, 0.0); glVertex3f( 100,-100, 100);
glTexCoord2f(0.0, 1.0); glVertex3f( 100,-100,-100);
glTexCoord2f(1.0, 0.0); glVertex3f( 100, 100, 100);
glTexCoord2f(1.0, 1.0); glVertex3f( 100, 100,-100);
glEnd();
sf::Texture::bind(textureManager.getTexture("StarsTop"), sf::Texture::Normalized);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(0.0, 0.0); glVertex3f(-100, 100, 100);
glTexCoord2f(0.0, 1.0); glVertex3f(-100,-100, 100);
glTexCoord2f(1.0, 0.0); glVertex3f( 100, 100, 100);
glTexCoord2f(1.0, 1.0); glVertex3f( 100,-100, 100);
glEnd();
sf::Texture::bind(textureManager.getTexture("StarsBottom"), sf::Texture::Normalized);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(1.0, 0.0); glVertex3f( 100,-100,-100);
glTexCoord2f(0.0, 0.0); glVertex3f(-100,-100,-100);
glTexCoord2f(1.0, 1.0); glVertex3f( 100, 100,-100);
glTexCoord2f(0.0, 1.0); glVertex3f(-100, 100,-100);
glEnd();
if (gameGlobalInfo)
{
//Render the background nebulas from the gameGlobalInfo. This ensures that all screens see the same background as it is replicated across clients.
for(int n=0; n<GameGlobalInfo::max_nebulas; n++)
{
sf::Texture::bind(textureManager.getTexture(gameGlobalInfo->nebula_info[n].textureName), sf::Texture::Pixels);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE);
glPushMatrix();
glRotatef(180, gameGlobalInfo->nebula_info[n].vector.x, gameGlobalInfo->nebula_info[n].vector.y, gameGlobalInfo->nebula_info[n].vector.z);
glColor4f(1,1,1,0.1);
glBegin(GL_TRIANGLE_STRIP);
glTexCoord2f(1.0, 0); glVertex3f( 100, 100, 100);
glTexCoord2f( 0, 0); glVertex3f( 100, 100,-100);
glTexCoord2f(1.0, 1.0); glVertex3f(-100, 100, 100);
glTexCoord2f( 0, 1.0); glVertex3f(-100, 100,-100);
glEnd();
glPopMatrix();
}
}
{
float lightpos1[4] = {0, 0, 0, 1.0};
glLightfv(GL_LIGHT1, GL_POSITION, lightpos1);
float lightpos0[4] = {20000, 20000, 20000, 1.0};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos0);
}
class RenderInfo
{
public:
RenderInfo(SpaceObject* obj, float d)
: object(obj), depth(d)
{}
SpaceObject* object;
float depth;
};
std::vector<std::vector<RenderInfo>> render_lists;
sf::Vector2f viewVector = sf::vector2FromAngle(camera_yaw);
float depth_cutoff_back = camera_position.z * -tanf((90+camera_pitch + camera_fov/2.0) / 180.0f * M_PI);
float depth_cutoff_front = camera_position.z * -tanf((90+camera_pitch - camera_fov/2.0) / 180.0f * M_PI);
if (camera_pitch - camera_fov/2.0 <= 0.0)
depth_cutoff_front = std::numeric_limits<float>::infinity();
if (camera_pitch + camera_fov/2.0 >= 180.0)
depth_cutoff_back = -std::numeric_limits<float>::infinity();
foreach(SpaceObject, obj, space_object_list)
{
float depth = sf::dot(viewVector, obj->getPosition() - sf::Vector2f(camera_position.x, camera_position.y));
if (depth + obj->getRadius() < depth_cutoff_back)
continue;
if (depth - obj->getRadius() > depth_cutoff_front)
continue;
if (depth > 0 && obj->getRadius() / depth < 1.0 / 500)
continue;
int render_list_index = std::max(0, int((depth + obj->getRadius()) / 25000));
while(render_list_index >= int(render_lists.size()))
render_lists.emplace_back();
render_lists[render_list_index].emplace_back(*obj, depth);
}
void templateAppInit(int width, int height) {
atexit(templateAppExit);
gfx = new GFX;
glViewport(0.0f, 0.0f, width, height);
gfx->set_matrix_mode(PROJECTION_MATRIX);
gfx->load_identity();
// Adjust "Field of View Y" angle for devices which has an aspect
// ratio which is wider than the origin iPhone (3:2). Devices which
// have a narrower aspect ratio (such as iPad) work fine, as is.
const float iPhoneOriginalWidth =320.0f;
const float iPhoneOriginalHeight=480.0f;
const float originalFovy=45.0f;
float fovy(originalFovy);
if (height*iPhoneOriginalWidth > width*iPhoneOriginalHeight) {
float h = (iPhoneOriginalHeight*0.5f) / tanf(originalFovy*0.5f*DEG_TO_RAD);
fovy = 2.0f * atan2f(((float)height)*0.5, h) * RAD_TO_DEG;
}
gfx->set_perspective(fovy,
(float)width / (float)height,
0.1f,
100.0f,
0.0f);
obj = new OBJ(OBJ_FILE, true);
for (auto objmesh=obj->objmesh.begin();
objmesh!=obj->objmesh.end(); ++objmesh) {
console_print("%s: %d: GL_TRIANGLES\n",
objmesh->name,
objmesh->objtrianglelist[0].n_indice_array);
/* Built-in method that implements the NvTriStrip library.
* For more information, check the obj.cpp source code in
* order to implement it inside your own apps.
*/
objmesh->optimize(128);
console_print("%s: %d: GL_TRIANGLE_STRIP\n",
objmesh->name,
objmesh->objtrianglelist[0].n_indice_array);
objmesh->build();
objmesh->free_vertex_data();
}
for (auto texture=obj->texture.begin();
texture!=obj->texture.end(); ++texture) {
(*texture)->build(obj->texture_path,
TEXTURE_MIPMAP | TEXTURE_16_BITS, // Automatically convert the texture to 16 bits.
TEXTURE_FILTER_2X,
0.0f);
}
for (auto objmaterial=obj->objmaterial.begin();
objmaterial!=obj->objmaterial.end(); ++objmaterial) {
objmaterial->build(new PROGRAM((char *)"default",
VERTEX_SHADER,
FRAGMENT_SHADER,
true,
true,
program_bind_attrib_location,
NULL));
objmaterial->set_draw_callback(material_draw_callback);
}
}