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

}
Ejemplo n.º 4
0
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;
	}
Ejemplo n.º 5
0
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;
}
Ejemplo n.º 6
0
//----------------------------------------------------------
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;
	}
};
Ejemplo n.º 7
0
// 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, &currentFrame, 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 );
}
Ejemplo n.º 8
0
//-------------------------------------------------------------
//!	@brief		: タンジェント
//!	@param[in]	: ラジアン値(F32)
//!	@return		: タンジェント値
//-------------------------------------------------------------
F32 Tan( F32 rad )
{
    return tanf(rad);
}
Ejemplo n.º 9
0
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);
}
Ejemplo n.º 10
0
//----------------------------------------------------------------//
float Cot ( float radians ) {
	return 1.0f / tanf ( radians );
}
Ejemplo n.º 11
0
//----------------------------------------------------------------//
float Tan ( float radians ) {
	return tanf ( radians );
}
Ejemplo n.º 12
0
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;
}
Ejemplo n.º 13
0
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);
}
Ejemplo n.º 14
0
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()
Ejemplo n.º 15
0
float Angle::getTangent() const {
    return tanf(getRadians());
}
Ejemplo n.º 16
0
void Functions::tan(Aurora::NWScript::FunctionContext &ctx) {
	ctx.getReturn() = tanf(ctx.getParams()[0].getFloat());
}
Ejemplo n.º 17
0
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);
}
Ejemplo n.º 18
0
float		stans(float x) {
    return (tanf(x));
}
Ejemplo n.º 19
0
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);
}
Ejemplo n.º 20
0
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() {
Ejemplo n.º 22
0
// 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;
	}
}
Ejemplo n.º 23
0
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);
}
Ejemplo n.º 24
0
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);
}
Ejemplo n.º 25
0
//----------------------------------------
float ofCamera::getImagePlaneDistance(ofRectangle viewport) const {
	return viewport.height / (2.0f * tanf(PI * fov / 360.0f));
}
Ejemplo n.º 26
0
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;
}
Ejemplo n.º 27
0
//函数名: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;
	}
}
Ejemplo n.º 28
0
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();
}
Ejemplo n.º 29
0
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);
    }
Ejemplo n.º 30
0
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);
    }
}