//! renders a antialiased, colored ring
void CGUIColorSelectDialog::buildColorRing( const core::dimension2d<u32> & dim, s32 supersample, const video::SColor& borderColor )
{
	const core::dimension2d<u32> d(dim.Width * supersample, dim.Height * supersample);
	video::CImage *RawTexture = new video::CImage(video::ECF_A8R8G8B8, d);

	RawTexture->fill ( 0x00808080 );

	const s32 radiusOut = ( d.Width / 2 ) - 4;
	const s32 fullR2 = radiusOut * radiusOut;

	video::SColor rgb(0xFF000000);
	video::SColorHSL hsl;
	hsl.Luminance = 0.5f;
	hsl.Saturation = 1.f;

	core::position2d<s32> p;
	for ( p.Y = -radiusOut;  p.Y <= radiusOut;  p.Y += 1  )
	{
		s32 y2 = p.Y * p.Y;

		for (p.X = -radiusOut; p.X <= radiusOut; p.X += 1)
		{
			s32 r2 = y2 + ( p.X * p.X );

			// test point in circle
			s32 testa = r2 - fullR2;

			if ( testa < 0 )
			{
				// dotproduct u ( x,y ) * v ( 1, 0 ) = cosinus(a)

				const f32 r = sqrtf((f32) r2);

				// normalize, dotproduct = xnorm
				const f32 xn = r == 0.f ? 0.f : -p.X * core::reciprocal(r);

				hsl.Hue = acosf(xn);
				if ( p.Y > 0 )
					hsl.Hue = (2.f * core::PI ) - hsl.Hue;

				hsl.Hue -= core::PI / 2.f;

				const f32 rTest = r / radiusOut;
/*
				if ( rTest < 0.25f )
				{
					hsl.Luminance = rTest / 0.25f;
					hsl.Saturation = 0.f;
					hsl.toRGB  ( rgb );
					*dst = rgb.color;
				}
				else
				if ( rTest < 0.4f )
				{
					hsl.Saturation = ( rTest - 0.25f ) / 0.15f;
					hsl.Luminance = 1.f - ( hsl.Saturation / 2.4f );
					hsl.Luminance = 0.5f;
					hsl.toRGB  ( rgb );
					// *dst = rgb.color;
				}
				else
				if ( rTest < 0.75f )
				{
					hsl.Luminance = 0.5f;
					hsl.Saturation = 1.f;
					hsl.toRGB  ( rgb );
					*dst = rgb.color;
				}
				else
				if ( rTest < 0.98f )
				{
					hsl.Luminance = 0.5f - ( ( rTest - 0.75f ) / 0.75f );
					hsl.Saturation = 1.f;
					hsl.toRGB  ( rgb );
					*dst = rgb.color;
				}
*/

				if ( rTest >= 0.5f )
				{
					hsl.Luminance = 0.5f;
					hsl.Saturation = 1.f;
					hsl.toRGB(rgb);

					if ( rTest <= 0.55f )
					{
						const u32 alpha = (u32) ( (rTest - 0.5f ) * ( 255.f / 0.05f ) );
						rgb.setAlpha(alpha);
					} 
					else if ( rTest >= 0.95f )
					{
						const u32 alpha = (u32) ( (rTest - 0.95f ) * ( 255.f / 0.05f ) );
						rgb.setAlpha(255-alpha);
					}
					else
						rgb.setAlpha(255);
					RawTexture->setPixel(4+p.X+radiusOut, 4+p.Y+radiusOut, rgb);
				}
			}
		}
	}

	RawTexture->unlock ();

	if ( supersample > 1 )
	{
		video::CImage * filter = new video::CImage(video::ECF_A8R8G8B8, dim );
		RawTexture->copyToScalingBoxFilter(filter);
		RawTexture->drop();
		RawTexture = filter;
	}

	video::IVideoDriver* driver = Environment->getVideoDriver();

	bool generateMipLevels = driver->getTextureCreationFlag(video::ETCF_CREATE_MIP_MAPS);
	driver->setTextureCreationFlag( video::ETCF_CREATE_MIP_MAPS, false);

	ColorRing.Texture = driver->addTexture ( "#colorring", RawTexture);
	RawTexture->drop();

	driver->setTextureCreationFlag(video::ETCF_CREATE_MIP_MAPS, generateMipLevels);
}
Esempio n. 2
0
    void RebuildStorageBlocks(bool bSpecificArea=false, StorageData *rebuildArea=NULL)
    {
        int startX = 0, startY = 0, endX = seperationUnits, endY = seperationUnits;
        if(!bSpecificArea)
        {
            StorageBlocks.ClearAll();
            storageBlockSize = curSize/float(seperationUnits);
            storageBlockSizeI = 1.0f/storageBlockSize;

            availableOffsetBlocks = (curSize.x > curSize.y) ? int(curSize.x*storageBlockSizeI.y) : int(curSize.y*storageBlockSizeI.x);
            availableOffsetBlocks -= seperationUnits;
        }
        else
        {
            UVSection &section = Sections[rebuildArea->item];

            Vect2 max;
            if(rebuildArea->bRot)   max.Set(rebuildArea->pos.x+section.height, rebuildArea->pos.y+section.width);
            else                    max.Set(rebuildArea->pos.x+section.width,  rebuildArea->pos.y+section.height);

            PosToBlock(rebuildArea->pos, startX, startY);
            PosToBlock(max, endX, endY, true);
        }

        for(int y=startY; y<endY; y++)
        {
            DWORD yPos = y*seperationUnits;
            for(int x=startX; x<endX; x++)
            {
                DWORD blockPos = yPos+x;

                //doing a 5-point check to speed things up rather than a full box/tri intersection
                Vect2 checkPoints[2];
                checkPoints[0].Set(float(x)*storageBlockSize.x, float(y)*storageBlockSize.y);
                checkPoints[1] = checkPoints[0]+storageBlockSize;

                for(int i=0; i<CurStorage.Num(); i++)
                {
                    StorageData &data = CurStorage[i];
                    UVSection &section = Sections[data.item];

                    Vect2 max;
                    if(data.bRot)   max.Set(data.pos.x+section.height, data.pos.y+section.width);
                    else            max.Set(data.pos.x+section.width,  data.pos.y+section.height);

                    if( checkPoints[0].x > max.x || data.pos.x > checkPoints[1].x ||
                        checkPoints[0].y > max.y || data.pos.y > checkPoints[1].y )
                    {
                        continue;
                    }

                    int numTris = section.Verts.Num()/3;

                    bool bFoundBlocker = false;

                    for(int j=0; j<numTris; j++)
                    {
                        int startVert = j*3;
                        Vect2 vPos1 = section.ProjectedVerts[startVert];
                        Vect2 vPos2 = section.ProjectedVerts[startVert+1];
                        Vect2 vPos3 = section.ProjectedVerts[startVert+2];

                        if(data.bRot)
                        {
                            vPos1.SwapVals();
                            vPos2.SwapVals();
                            vPos3.SwapVals();

                            vPos1.x = section.height-vPos1.x;
                            vPos2.x = section.height-vPos2.x;
                            vPos3.x = section.height-vPos3.x;
                        }

                        vPos1 += data.pos;
                        vPos2 += data.pos;
                        vPos3 += data.pos;

                        if( LineCheckForBoxOnIsle2(checkPoints[0], checkPoints[1], vPos1, vPos2) ||
                            LineCheckForBoxOnIsle2(checkPoints[0], checkPoints[1], vPos2, vPos3) ||
                            LineCheckForBoxOnIsle2(checkPoints[0], checkPoints[1], vPos3, vPos1) )
                        {
                            StorageBlocks.Set(blockPos);
                            bFoundBlocker = true;
                            break;
                        }

                        Vect2 norm1 = (vPos1-checkPoints[0]).Norm();
                        Vect2 norm2 = (vPos2-checkPoints[0]).Norm();
                        Vect2 norm3 = (vPos3-checkPoints[0]).Norm();
                        float checkVal = acosf(norm1.Dot(norm2)) +
                                         acosf(norm2.Dot(norm3)) +
                                         acosf(norm3.Dot(norm1));

                        if(fabsf(checkVal-(M_PI*2.0f)) < 0.01f)
                        {
                            StorageBlocks.Set(blockPos);
                            bFoundBlocker = true;
                            break;
                        }
                    }

                    if(bFoundBlocker)
                        break;
                }
            }
        }

        /*String fileName;
        fileName << TEXT("floong") << IntString(storageBlockRebuildCount) << TEXT(".txt");
        XFile wong(fileName, XFILE_WRITE, XFILE_CREATEALWAYS);
        for(int y=0; y<seperationUnits; y++)
        {
            DWORD yPos = y*seperationUnits;
            for(int x=0; x<seperationUnits; x++)
            {
                wong.WriteAsUTF8(IntString(StorageBlocks[yPos+x] != 0));
            }

            wong.WriteAsUTF8(TEXT("\r\n"));
        }
        ++storageBlockRebuildCount;*/
    }
Esempio n. 3
0
float __racosd(float x)
{
  return (acosf(x) * 57.29577951308232F);
}
Esempio n. 4
0
	//	カプセル描画
	void	DrawCapsule( LPDIRECT3DDEVICE9 d3dd, CONST D3DXVECTOR3 &p0, CONST D3DXVECTOR3 &p1, FLOAT r, D3DCOLOR c )
	{
		CONST INT N = 36;
		CONST INT L0 = 0;
		CONST INT L1 = N + 1;
		CONST INT L2 = L1 + N + 1;

		CONST INT L3 = L2 + N * 2;
		CONST INT L4 = L3 + N + 1;
		CONST INT L5 = L4 + N + 1;
		CONST INT L6 = L5 + N + 1;

		static _VB vb(d3dd, sizeof(D3DXVECTOR3)* ((N + 1) * 2 + N * 2 + (N + 1) * 4));
		D3DXVECTOR3 *p;
		vb->Lock(0, 0, (void **)&p, 0);
		D3DXVECTOR3 up = p0 - p1;
		CONST FLOAT L = D3DXVec3Length(&up);
		for (int i = 0; i < N; i++)
		{
			p[L0 + i] = D3DXVECTOR3(r * sinf(360.0f / N * 0.01745f * i), L / 2.0f, r * cosf(360.0f / N * 0.01745f * i));
			p[L1 + i] = D3DXVECTOR3(r * sinf(360.0f / N * 0.01745f * i), -L / 2.0f, r * cosf(360.0f / N * 0.01745f * i));
			if (i % (N / 4) == 0)
			{
				p[L2 + 2 * i] = p[L0 + i];
				p[L2 + 2 * i + 1] = p[L1 + i];
			}
			p[L3 + i] = D3DXVECTOR3(r * cosf(180.0f / N * 0.01745f * i), r * sinf(180.0f / N * 0.01745f * i) + L / 2.0f, 0);
			p[L4 + i] = D3DXVECTOR3(0, r * sinf(180.0f / N * 0.01745f * i) + L / 2.0f, r * cosf(180.0f / N * 0.01745f * i));
			p[L5 + i] = D3DXVECTOR3(r * cosf(180.0f / N * 0.01745f * i), -r * sinf(180.0f / N * 0.01745f * i) - L / 2.0f, 0);
			p[L6 + i] = D3DXVECTOR3(0, -r * sinf(180.0f / N * 0.01745f * i) - L / 2.0f, r * cosf(180.0f / N * 0.01745f * i));
		}
		p[L0 + N] = p[L0];
		p[L1 + N] = p[L1];
		p[L3 + N] = D3DXVECTOR3(-r, L / 2.0f, 0);
		p[L4 + N] = D3DXVECTOR3(0, L / 2.0f, -r);
		p[L5 + N] = D3DXVECTOR3(-r, L / 2.0f, 0);
		p[L6 + N] = D3DXVECTOR3(0, L / 2.0f, -r);

		vb->Unlock();

		D3DXVECTOR3 axis;
		D3DXVec3Cross(&axis, &D3DXVECTOR3(0, 1, 0), &up);
		D3DXVec3Normalize(&axis, &axis);
		FLOAT angle = acosf(D3DXVec3Dot(&up, &D3DXVECTOR3(0, 1, 0)) / D3DXVec3Length(&up));
		D3DXMATRIX M;
		D3DXMatrixRotationAxis(&M, &axis, angle);
		D3DXVECTOR3 g = (p0 + p1) / 2;
		M._41 = g.x;
		M._42 = g.y;
		M._43 = g.z;

		D3DMATERIAL9 m = { 0 };
		m.Diffuse = m.Ambient = D3DXCOLOR(c);
		d3dd->SetMaterial(&m);

		d3dd->SetFVF(D3DFVF_XYZ);
		d3dd->SetStreamSource(0, vb, 0, sizeof(D3DXVECTOR3));
		d3dd->SetTransform(D3DTS_WORLD, &M);
		d3dd->DrawPrimitive(D3DPT_LINESTRIP, L0, N);
		d3dd->DrawPrimitive(D3DPT_LINESTRIP, L1, N);
		d3dd->DrawPrimitive(D3DPT_LINELIST, L2, N);
		d3dd->DrawPrimitive(D3DPT_LINESTRIP, L3, N);
		d3dd->DrawPrimitive(D3DPT_LINESTRIP, L4, N);
		d3dd->DrawPrimitive(D3DPT_LINESTRIP, L5, N);
		d3dd->DrawPrimitive(D3DPT_LINESTRIP, L6, N);
	}
Esempio n. 5
0
Quaternion Quaternion::Slerp(const Quaternion& rhs, float t) const
{
    // Use fast approximation for Emscripten builds
#ifdef __EMSCRIPTEN__
    float angle = DotProduct(rhs);
    float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
    if (angle < 0.f)
    {
        angle = -angle;
        sign = -1.f;
    }

    float a;
    float b;
    if (angle < 0.999f) // perform spherical linear interpolation.
    {
        // angle = acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
        angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f;
        float ta = t*angle;
        // Manually compute the two sines by using a very rough approximation.
        float ta2 = ta*ta;
        b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta;
        a = angle - ta;
        float a2 = a*a;
        a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a;
    }
    else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
    {
        a = 1.f - t;
        b = t;
    }
    // Lerp and renormalize.
    return (*this * (a * sign) + rhs * b).Normalized();
#else
    // Favor accuracy for native code builds
    float cosAngle = DotProduct(rhs);
    float sign = 1.0f;
    // Enable shortest path rotation
    if (cosAngle < 0.0f)
    {
        cosAngle = -cosAngle;
        sign = -1.0f;
    }

    float angle = acosf(cosAngle);
    float sinAngle = sinf(angle);
    float t1, t2;

    if (sinAngle > 0.001f)
    {
        float invSinAngle = 1.0f / sinAngle;
        t1 = sinf((1.0f - t) * angle) * invSinAngle;
        t2 = sinf(t * angle) * invSinAngle;
    }
    else
    {
        t1 = 1.0f - t;
        t2 = t;
    }

    return *this * t1 + (rhs * sign) * t2;
#endif
}
Esempio n. 6
0
HRESULT KG3DMeshBone::AddSubMesh(D3DXVECTOR3 v1, D3DXVECTOR3 v2, TCHAR* szBoneName, DWORD dwType)
{
    HRESULT hResult  = E_FAIL;

    _SubMeshInfo subMeshInfo;
    D3DXVECTOR3  vUp = D3DXVECTOR3(0.0f, 1.0f, 0.0f);
    D3DXVECTOR3  vDir = v2 - v1;
    D3DXVECTOR3  vVer;
    D3DXVECTOR3  vHor;
    D3DXMATRIX   matRotation;
    FLOAT        fAngel = 0;
    FLOAT        fLen   = 0;

    ASSERT(szBoneName);

    fLen = D3DXVec3Length(&vDir);
    D3DXVec3Normalize(&vDir, &vDir);
    fAngel = D3DXVec3Dot(&vUp, &vDir);

    if (abs(acosf(fAngel)) <= 0.05f)
    {
        vVer = D3DXVECTOR3(1.0f, 0.0f, 0.0f);
        vHor = D3DXVECTOR3(0.0f, 0.0f, 1.0f);
    }
    else
    {
        D3DXMatrixRotationY(&matRotation, D3DX_PI /2);
        D3DXVec3TransformNormal(&vVer, &vDir, &matRotation);

        D3DXVec3Cross(&vHor, &vDir, &vVer);
        D3DXVec3Cross(&vVer, &vDir, &vHor);
    }

    D3DXVec3Normalize(&vVer, &vVer);
    D3DXVec3Normalize(&vHor, &vHor);

    switch (dwType)
    {
    case 1 :
        vVer *= fLen / 2;
        vHor *= fLen / 2;
        subMeshInfo.vPosition[0] = v2;
        subMeshInfo.vPosition[1] = v1 + (vDir * fLen) - vHor - vVer;
        subMeshInfo.vPosition[2] = v1 + (vDir * fLen) + vHor - vVer;
        subMeshInfo.vPosition[3] = v1 + (vDir * fLen) + vHor + vVer;
        subMeshInfo.vPosition[4] = v1 + (vDir * fLen) - vHor + vVer;
        subMeshInfo.vPosition[5] = v1;
        break;
    case 0 :
        vVer *= fLen / 15;
        vHor *= fLen / 15;
        subMeshInfo.vPosition[0] = v2;
        subMeshInfo.vPosition[1] = v1 + (vDir * fLen / 15) - vHor - vVer;
        subMeshInfo.vPosition[2] = v1 + (vDir * fLen / 15) + vHor - vVer;
        subMeshInfo.vPosition[3] = v1 + (vDir * fLen / 15) + vHor + vVer;
        subMeshInfo.vPosition[4] = v1 + (vDir * fLen / 15) - vHor + vVer;
        subMeshInfo.vPosition[5] = v1;
        break;
    case 2 :
        vVer *= fLen / 15;
        vHor *= fLen / 15;
        subMeshInfo.vPosition[0] = v2;
        subMeshInfo.vPosition[1] = v1 + (vDir * fLen / 2) - vHor - vVer;
        subMeshInfo.vPosition[2] = v1 + (vDir * fLen / 2) + vHor - vVer;
        subMeshInfo.vPosition[3] = v1 + (vDir * fLen / 2) + vHor + vVer;
        subMeshInfo.vPosition[4] = v1 + (vDir * fLen / 2) - vHor + vVer;
        subMeshInfo.vPosition[5] = v1;
        break;
    default :
        KG_PROCESS_ERROR(0);
        break;
    }

    subMeshInfo.dwIndex[0]   = 0;
    subMeshInfo.dwIndex[1]   = 3;
    subMeshInfo.dwIndex[2]   = 4;

    subMeshInfo.dwIndex[3]   = 0;
    subMeshInfo.dwIndex[4]   = 2;
    subMeshInfo.dwIndex[5]   = 3;

    subMeshInfo.dwIndex[6]   = 0;
    subMeshInfo.dwIndex[7]   = 1;
    subMeshInfo.dwIndex[8]   = 2;

    subMeshInfo.dwIndex[9]   = 0;
    subMeshInfo.dwIndex[10]  = 4;
    subMeshInfo.dwIndex[11]  = 1;

   /* subMeshInfo.dwIndex[12]  = 1;
    subMeshInfo.dwIndex[13]  = 4;
    subMeshInfo.dwIndex[14]  = 2;

    subMeshInfo.dwIndex[15]  = 2;
    subMeshInfo.dwIndex[16]  = 4;
    subMeshInfo.dwIndex[17]  = 3;*/

    subMeshInfo.dwIndex[12]   = 5;
    subMeshInfo.dwIndex[13]   = 4;
    subMeshInfo.dwIndex[14]   = 3;

    subMeshInfo.dwIndex[15]   = 5;
    subMeshInfo.dwIndex[16]   = 3;
    subMeshInfo.dwIndex[17]   = 2;

    subMeshInfo.dwIndex[18]   = 5;
    subMeshInfo.dwIndex[19]   = 2;
    subMeshInfo.dwIndex[20]   = 1;

    subMeshInfo.dwIndex[21]   = 5;
    subMeshInfo.dwIndex[22]   = 1;
    subMeshInfo.dwIndex[23]   = 4;

    subMeshInfo.dwType = dwType;
    strncpy(subMeshInfo.szBoneName, szBoneName, sizeof(subMeshInfo.szBoneName));
    m_vecSubMeshInfo.push_back(subMeshInfo);

    hResult = S_OK;
Exit0:
    return hResult;
}
Esempio n. 7
0
NS_CC_BEGIN

void ccVertexLineToPolygon(Vec2 *points, float stroke, Vec2 *vertices, unsigned int offset, unsigned int nuPoints)
{
    nuPoints += offset;
    if(nuPoints<=1) return;

    stroke *= 0.5f;

    unsigned int idx;
    unsigned int nuPointsMinus = nuPoints-1;

    for(unsigned int i = offset; i<nuPoints; i++)
    {
        idx = i*2;
        Vec2 p1 = points[i];
        Vec2 perpVector;

        if(i == 0)
            perpVector = (p1 - points[i+1]).getNormalized().getPerp();
        else if(i == nuPointsMinus)
            perpVector = (points[i-1] - p1).getNormalized().getPerp();
        else
        {
            Vec2 p2 = points[i+1];
            Vec2 p0 = points[i-1];

            Vec2 p2p1 = (p2 - p1).getNormalized();
            Vec2 p0p1 = (p0 - p1).getNormalized();

            // Calculate angle between vectors
            float angle = acosf(p2p1.dot(p0p1));

            if(angle < CC_DEGREES_TO_RADIANS(70))
                perpVector = p2p1.getMidpoint(p0p1).getNormalized().getPerp();
            else if(angle < CC_DEGREES_TO_RADIANS(170))
                perpVector = p2p1.getMidpoint(p0p1).getNormalized();
            else
                perpVector = (p2 - p0).getNormalized().getPerp();
        }
        perpVector = perpVector * stroke;

        vertices[idx].set(p1.x + perpVector.x, p1.y + perpVector.y);
        vertices[idx + 1].set(p1.x - perpVector.x, p1.y - perpVector.y);

    }

    // Validate vertexes
    offset = (offset==0) ? 0 : offset-1;
    for(unsigned int i = offset; i<nuPointsMinus; i++)
    {
        idx = i*2;
        const unsigned int idx1 = idx+2;

        Vec2 p1 = vertices[idx];
        Vec2 p2 = vertices[idx+1];
        Vec2 p3 = vertices[idx1];
        Vec2 p4 = vertices[idx1+1];

        float s;
        //BOOL fixVertex = !ccpLineIntersect(Vec2(p1.x, p1.y), Vec2(p4.x, p4.y), Vec2(p2.x, p2.y), Vec2(p3.x, p3.y), &s, &t);
        bool fixVertex = !ccVertexLineIntersect(p1.x, p1.y, p4.x, p4.y, p2.x, p2.y, p3.x, p3.y, &s);
        if(!fixVertex)
            if (s<0.0f || s>1.0f)
                fixVertex = true;

        if(fixVertex)
        {
            vertices[idx1] = p4;
            vertices[idx1+1] = p3;
        }
    }
}
Esempio n. 8
0
int test_float(int argc, char *argv[])
{
	int ret = 0;

	printf("\n--- SINGLE PRECISION TESTS ---\n");
	printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");

	float f1 = 1.55f;

	float sinf_zero = sinf(0.0f);
	float sinf_one = sinf(1.0f);
	float sqrt_two = sqrt(2.0f);

	if (sinf_zero == 0.0f) {
		printf("\t success: sinf(0.0f) == 0.0f\n");

	} else {
		printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
		ret = -4;
	}

	fflush(stdout);

	if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
		printf("\t success: sinf(1.0f) == 0.84147f\n");

	} else {
		printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
		ret = -1;
	}

	fflush(stdout);

	float asinf_one = asinf(1.0f);

	if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
		printf("\t success: asinf(1.0f) == 1.57079f\n");

	} else {
		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
		ret = -1;
	}

	fflush(stdout);

	float cosf_one = cosf(1.0f);

	if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
		printf("\t success: cosf(1.0f) == 0.54030f\n");

	} else {
		printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
		ret = -1;
	}

	fflush(stdout);


	float acosf_one = acosf(1.0f);

	if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
		printf("\t success: acosf(1.0f) == 0.0f\n");

	} else {
		printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
		ret = -1;
	}

	fflush(stdout);


	float sinf_zero_one = sinf(0.1f);

	if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
		printf("\t success: sinf(0.1f) == 0.09983f\n");

	} else {
		printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
		ret = -2;
	}

	if (sqrt_two == 1.41421356f) {
		printf("\t success: sqrt(2.0f) == 1.41421f\n");

	} else {
		printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
		ret = -3;
	}

	float atan2f_ones = atan2(1.0f, 1.0f);

	if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) {
		printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");

	} else {
		printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
		ret = -4;
	}

	char sbuf[30];
	sprintf(sbuf, "%8.4f", 0.553415f);

	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
		printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
	} else {
		printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \"  0.5534\", result: %s\n", sbuf);
		ret = -5;
	}

	sprintf(sbuf, "%8.4f", -0.553415f);

	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
		printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double)-0.553415f);
	} else {
		printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
		ret = -6;
	}





	printf("\n--- DOUBLE PRECISION TESTS ---\n");

	double d1 = 1.0111;
	double d2 = 2.0;

	double d1d2 = d1 * d2;

	if (d1d2 == 2.022200000000000219557705349871) {
		printf("\t success: 1.0111 * 2.0 == 2.0222\n");

	} else {
		printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
		ret = -7;
	}

	fflush(stdout);

	// Assign value of f1 to d1
	d1 = f1;

	if (f1 == (float)d1) {
		printf("\t success: (float) 1.55f == 1.55 (double)\n");

	} else {
		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
		ret = -8;
	}

	fflush(stdout);


	double sin_zero = sin(0.0);
	double sin_one = sin(1.0);
	double atan2_ones = atan2(1.0, 1.0);

	if (sin_zero == 0.0) {
		printf("\t success: sin(0.0) == 0.0\n");

	} else {
		printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
		ret = -9;
	}

	if (sin_one == 0.841470984807896504875657228695) {
		printf("\t success: sin(1.0) == 0.84147098480\n");

	} else {
		printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
		ret = -10;
	}

	if (atan2_ones != 0.785398) {
		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");

	} else {
		printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
		ret = -11;
	}

	printf("\t testing pow() with magic value\n");
	printf("\t   (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
	fflush(stdout);
	usleep(20000);
	double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
	printf("\t success: result: %8.4f\n", (double)powres);

	sprintf(sbuf, "%8.4f", 0.553415);

	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
		printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
	} else {
		printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \"  0.5534\", result: %s\n", sbuf);
		ret = -12;
	}

	sprintf(sbuf, "%8.4f", -0.553415);

	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
		&& sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
		printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
	} else {
		printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
		ret = -13;
	}


	if (ret == 0) {
		printf("\n SUCCESS: All float and double tests passed.\n");

	} else {
		printf("\n FAIL: One or more tests failed.\n");
	}

	printf("\n");

	return ret;
}
Esempio n. 9
0
static void getEstimatedAttitude(void)
{
    int32_t axis;
    int32_t accMag = 0;
    static t_fp_vector EstM;
    static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
    static float accLPF[3];
    static uint32_t previousT;
    uint32_t currentT = micros();
    uint32_t deltaT;
    float scale, deltaGyroAngle[3];
    deltaT = currentT - previousT;
    scale = deltaT * gyro.scale;
    previousT = currentT;

    // Initialization
    for (axis = 0; axis < 3; axis++) {
        deltaGyroAngle[axis] = gyroADC[axis] * scale;
        if (cfg.acc_lpf_factor > 0) {
            accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
            accSmooth[axis] = accLPF[axis];
        } else {
            accSmooth[axis] = accADC[axis];
        }
        accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
    }
    accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);

    rotateV(&EstG.V, deltaGyroAngle);
    if (sensors(SENSOR_MAG)) {
        rotateV(&EstM.V, deltaGyroAngle);
    } else {
        rotateV(&EstN.V, deltaGyroAngle);
        normalizeV(&EstN.V, &EstN.V);
    }

    // Apply complimentary filter (Gyro drift correction)
    // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
    // To do that, we just skip filter, as EstV already rotated by Gyro
    if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
        for (axis = 0; axis < 3; axis++)
            EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
    }

    if (sensors(SENSOR_MAG)) {
        for (axis = 0; axis < 3; axis++)
            EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
    }

    f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);

    // Attitude of the estimated vector
    anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
    anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
    angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
    angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));

    if (sensors(SENSOR_MAG))
        heading = calculateHeading(&EstM);
    else
        heading = calculateHeading(&EstN);

    acc_calc(deltaT); // rotate acc vector into earth frame

    if (cfg.throttle_correction_value) {

        float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);

        if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
            throttleAngleCorrection = 0;
        } else {
            int angle = lrintf(acosf(cosZ) * throttleAngleScale);
            if (angle > 900)
                angle = 900;
            throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
        }

    }
}
Esempio n. 10
0
void GeometryGenerator::CreateGeosphere(float radius, UINT numSubdivisions, MeshData& meshData)
{
	numSubdivisions = MathHelper::Min(numSubdivisions, 5u);

	const float X = 0.525731f; 
	const float Z = 0.850651f;

	glm::vec3 pos[12] = 
	{
		glm::vec3(-X, 0.0f, Z),  glm::vec3(X, 0.0f, Z),  
		glm::vec3(-X, 0.0f, -Z), glm::vec3(X, 0.0f, -Z),    
		glm::vec3(0.0f, Z, X),   glm::vec3(0.0f, Z, -X), 
		glm::vec3(0.0f, -Z, X),  glm::vec3(0.0f, -Z, -X),    
		glm::vec3(Z, X, 0.0f),   glm::vec3(-Z, X, 0.0f), 
		glm::vec3(Z, -X, 0.0f),  glm::vec3(-Z, -X, 0.0f)
	};

	DWORD k[60] = 
	{
		1,4,0,  4,9,0,  4,5,9,  8,5,4,  1,8,4,    
		1,10,8, 10,3,8, 8,3,5,  3,2,5,  3,7,2,    
		3,10,7, 10,6,7, 6,11,7, 6,0,11, 6,1,0, 
		10,1,6, 11,0,9, 2,11,9, 5,2,9,  11,2,7 
	};

	meshData.Vertices.resize(12);
	meshData.Indices.resize(60);

	for(UINT i = 0; i < 12; ++i)
		meshData.Vertices[i].Position = pos[i];

	for(UINT i = 0; i < 60; ++i)
		meshData.Indices[i] = k[i];

	for(UINT i = 0; i < numSubdivisions; ++i)
		Subdivide(meshData);

	for(UINT i = 0; i < meshData.Vertices.size(); ++i)
	{
		glm::vec3 n = glm::normalize(meshData.Vertices[i].Position);

		glm::vec3 p = radius*n;

		meshData.Vertices[i].Position = p;
		meshData.Vertices[i].Normal = n;

		float theta = MathHelper::AngleFromXY(
			meshData.Vertices[i].Position.x, 
			meshData.Vertices[i].Position.z);

		float phi = acosf(meshData.Vertices[i].Position.y / radius);

		meshData.Vertices[i].TexC.x = theta/(XM_PI * 2.0f);
		meshData.Vertices[i].TexC.y = phi/XM_PI;

		meshData.Vertices[i].TangentU.x = -radius*sinf(phi)*sinf(theta);
		meshData.Vertices[i].TangentU.y = 0.0f;
		meshData.Vertices[i].TangentU.z = +radius*sinf(phi)*cosf(theta);

		glm::vec3 T = meshData.Vertices[i].TangentU;
		meshData.Vertices[i].TangentU = glm::normalize(T);
	}
}
f32 wsArcCos(f32 val) {
    WS_PROFILE();
    f32 my = acosf(val)*RAD_TO_DEG;

    return my;
}
Esempio n. 12
0
template <typename PointInT, typename GradientT, typename PointOutT> void
pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT (
    const PointCloudIn &cloud, const PointCloudGradient &gradient,
    int p_idx, float radius, const std::vector<int> &indices,
    const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor)
{
    if (indices.empty ())
    {
        PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n");
        return;
    }

    // Determine the number of bins to use based on the size of rift_descriptor
    int nr_distance_bins = static_cast<int> (rift_descriptor.rows ());
    int nr_gradient_bins = static_cast<int> (rift_descriptor.cols ());

    // Get the center point
    pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap ();

    // Compute the RIFT descriptor
    rift_descriptor.setZero ();
    for (size_t idx = 0; idx < indices.size (); ++idx)
    {
        // Compute the gradient magnitude and orientation (relative to the center point)
        pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap ();
        Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient.points[indices[idx]].gradient[0]));

        float gradient_magnitude = gradient_vector.norm ();
        float gradient_angle_from_center = acosf (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude);
        if (!pcl_isfinite (gradient_angle_from_center))
            gradient_angle_from_center = 0.0;

        // Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins
        const float eps = std::numeric_limits<float>::epsilon ();
        float d = static_cast<float> (nr_distance_bins) * sqrtf (sqr_distances[idx]) / (radius + eps);
        float g = static_cast<float> (nr_gradient_bins) * gradient_angle_from_center / (static_cast<float> (M_PI) + eps);

        // Compute the bin indices that need to be updated
        int d_idx_min = (std::max)(static_cast<int> (ceil (d - 1)), 0);
        int d_idx_max = (std::min)(static_cast<int> (floor (d + 1)), nr_distance_bins - 1);
        int g_idx_min = static_cast<int> (ceil (g - 1));
        int g_idx_max = static_cast<int> (floor (g + 1));

        // Update the appropriate bins of the histogram
        for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx)
        {
            // Because gradient orientation is cyclical, out-of-bounds values must wrap around
            int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins);

            for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
            {
                // To avoid boundary effects, use linear interpolation when updating each bin
                float w = (1.0f - fabsf (d - static_cast<float> (d_idx))) * (1.0f - fabsf (g - static_cast<float> (g_idx)));

                rift_descriptor (d_idx, g_idx_wrapped) += w * gradient_magnitude;
            }
        }
    }

    // Normalize the RIFT descriptor to unit magnitude
    rift_descriptor.normalize ();
}
float Vector2::angle(Vector2 &other) {
	return acosf(dotProduct(other));
}
Esempio n. 14
0
float acos( float a ){
	ASSERT( a >= -1.f );
	ASSERT( a <= 1.f );
	return acosf( a ) * TO_DEG;
}
Esempio n. 15
0
//-----------------------------------------------------------------------------
bool CAI_PlaneSolver::GenerateCircleObstacleSuggestions( const AILocalMoveGoal_t &moveGoal, float probeDist )
{
	bool result = false;
	Vector npcLoc = m_pNpc->WorldSpaceCenter();
	Vector mins, maxs;

	m_pNpc->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs );
	float radiusNpc = (mins.AsVector2D() - maxs.AsVector2D()).Length() * 0.5;
	
	for ( int i = 0; i < m_Obstacles.Count(); i++ )
	{
		CBaseEntity *pObstacleEntity = NULL;

		float zDistTooFar;
		if ( m_Obstacles[i].hEntity && m_Obstacles[i].hEntity->CollisionProp() )
		{
			pObstacleEntity = m_Obstacles[i].hEntity.Get();

			if( pObstacleEntity == moveGoal.pMoveTarget && (pObstacleEntity->IsNPC() || pObstacleEntity->IsPlayer()) )
			{
				// HEY! I'm trying to avoid the very thing I'm trying to get to. This will make we wobble like a drunk as I approach. Don't do it.
				continue;
			}

			pObstacleEntity->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs );
			zDistTooFar = ( maxs.z - mins.z ) * 0.5 + GetNpc()->GetHullHeight() * 0.5;
		}
		else
			zDistTooFar = GetNpc()->GetHullHeight();
			
		if ( fabs( m_Obstacles[i].center.z - npcLoc.z ) > zDistTooFar )
			continue;

		Vector vecToNpc 		= npcLoc - m_Obstacles[i].center;
		vecToNpc.z = 0;
		float distToObstacleSq 	= sq(vecToNpc.x) + sq(vecToNpc.y);
		float radius = m_Obstacles[i].radius + radiusNpc;

		if ( distToObstacleSq > 0.001 && distToObstacleSq < sq( radius + probeDist ) )
		{
			Vector vecToObstacle = vecToNpc * -1;
			float distToObstacle = VectorNormalize( vecToObstacle );
			float weight;
			float arc;
			float radiusSq = sq(radius);

			float flDot = DotProduct( vecToObstacle, moveGoal.dir );

			// Don't steer around to avoid obstacles we've already passed, unless we're right up against them.
			// That is, do this computation without the probeDist added in.
			if( flDot < 0.0f && distToObstacleSq > radiusSq )
			{
				continue;
			}

			if ( radiusSq < distToObstacleSq )
			{
				Vector vecTangent;
				float distToTangent = FastSqrt( distToObstacleSq - radiusSq );

				float oneOverDistToObstacleSq = 1 / distToObstacleSq;

				vecTangent.x = ( -distToTangent * vecToNpc.x + radius * vecToNpc.y ) * oneOverDistToObstacleSq;
				vecTangent.y = ( -distToTangent * vecToNpc.y - radius * vecToNpc.x ) * oneOverDistToObstacleSq;
				vecTangent.z = 0;

				float cosHalfArc = vecToObstacle.Dot( vecTangent );
				arc = RAD2DEG(acosf( cosHalfArc )) * 2.0;
				weight = 1.0 - (distToObstacle - radius) / probeDist;
				if ( weight > 0.75 )
					arc += (arc * 0.5) * (weight - 0.75) / 0.25;
				
				Assert( weight >= 0.0 && weight <= 1.0 );

#if DEBUG_OBSTACLES
				// -------------------------
				Msg( "Adding arc %f, w %f\n", arc, weight );

				Vector pointTangent = npcLoc + ( vecTangent * distToTangent );
					
				NDebugOverlay::Line( npcLoc - Vector( 0, 0, 64 ), npcLoc + Vector(0,0,64), 0,255,0, false, 0.1 );
				NDebugOverlay::Line( center - Vector( 0, 0, 64 ), center + Vector(0,0,64), 0,255,0, false, 0.1 );
				NDebugOverlay::Line( pointTangent - Vector( 0, 0, 64 ), pointTangent + Vector(0,0,64), 0,255,0, false, 0.1 );
				
				NDebugOverlay::Line( npcLoc + Vector(0,0,64), center + Vector(0,0,64), 0,0,255, false, 0.1 );
				NDebugOverlay::Line( center + Vector(0,0,64), pointTangent + Vector(0,0,64), 0,0,255, false, 0.1 );
				NDebugOverlay::Line( pointTangent + Vector(0,0,64), npcLoc + Vector(0,0,64), 0,0,255, false, 0.1 );
#endif
			}
			else
			{
				arc = 210;
				weight = 1.0;
			}

			if ( m_Obstacles[i].hEntity != NULL )
			{
				weight = AdjustRegulationWeight( m_Obstacles[i].hEntity, weight );
			}
			
			AI_MoveSuggestion_t suggestion( m_Obstacles[i].type, weight, UTIL_VecToYaw(vecToObstacle), arc );
			m_Solver.AddRegulation( suggestion );
			result = true;
		}
	}
	
	m_Obstacles.RemoveAll();
	return result;

}
Esempio n. 16
0
// only defined for 0<=t<=1
static float console_scale_func(float t)
{
	//return t;
	return sinf(acosf(1.0f-t));
}
Esempio n. 17
0
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
{
  pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();

  const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
                                frames_->points[index].x_axis[1],
                                frames_->points[index].x_axis[2]);
  //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
  const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
                                frames_->points[index].z_axis[1],
                                frames_->points[index].z_axis[2]);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal(nn_dists[ne], 0.0f))
      continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    // ----- Compute current neighbour polar coordinates -----

    // Get distance between the neighbour and the origin
    float r = std::sqrt (nn_dists[ne]);

    // Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    // Normalize to compute the dot product
    proj.normalize ();

    // Compute the angle between the projection and the x axis in the interval [0,360]
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));

    /// Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    /// Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
    {
      if (r <= radii_interval_[rad])
      {
        j = rad - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
    {
      if (theta <= theta_divisions_[ang])
      {
        k = ang - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
    {
      if (phi <= phi_divisions_[ang])
      {
        l = ang - 1;
        break;
      }
    }

    /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_didtances;
    float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
    /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
                                                   (k*radius_bins_) +
                                                   j];

    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (w != w)
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondant Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour
}
Esempio n. 18
0
int lpc_to_lsp (float *a, int order, float *freq, int nb, float delta)
/*  float *a 		     	lpc coefficients			*/
/*  int order			order of LPC coefficients (10) 		*/
/*  float *freq 	      	LSP frequencies in radians      	*/
/*  int nb			number of sub-intervals (4) 		*/
/*  float delta			grid spacing interval (0.02) 		*/
{
    float psuml,psumr,psumm,temp_xr,xl,xr,xm = 0;
    float temp_psumr;
    int i,j,m,flag,k;
    float *px;                	/* ptrs of respective P'(z) & Q'(z)	*/
    float *qx;
    float *p;
    float *q;
    float *pt;                	/* ptr used for cheb_poly_eval()
				   whether P' or Q' 			*/
    int roots=0;              	/* number of roots found 	        */
    float Q[order + 1];
    float P[order + 1];

    flag = 1;                	
    m = order/2;            	/* order of P'(z) & Q'(z) polynimials 	*/

    /* Allocate memory space for polynomials */

    /* determine P'(z)'s and Q'(z)'s coefficients where
      P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */

    px = P;                      /* initilaise ptrs */
    qx = Q;
    p = px;
    q = qx;
    *px++ = 1.0;
    *qx++ = 1.0;
    for(i=1;i<=m;i++){
	*px++ = a[i]+a[order+1-i]-*p++;
	*qx++ = a[i]-a[order+1-i]+*q++;
    }
    px = P;
    qx = Q;
    for(i=0;i<m;i++){
	*px = 2**px;
	*qx = 2**qx;
	 px++;
	 qx++;
    }
    px = P;             	/* re-initialise ptrs 			*/
    qx = Q;

    /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
    Keep alternating between the two polynomials as each zero is found 	*/

    xr = 0;             	/* initialise xr to zero 		*/
    xl = 1.0;               	/* start at point xl = 1 		*/


    for(j=0;j<order;j++){
	if(j%2)            	/* determines whether P' or Q' is eval. */
	    pt = qx;
	else
	    pt = px;

	psuml = cheb_poly_eva(pt,xl,order);	/* evals poly. at xl 	*/
	flag = 1;
	while(flag && (xr >= -1.0)){
	    xr = xl - delta ;                  	/* interval spacing 	*/
	    psumr = cheb_poly_eva(pt,xr,order);/* poly(xl-delta_x) 	*/
	    temp_psumr = psumr;
	    temp_xr = xr;

        /* if no sign change increment xr and re-evaluate
           poly(xr). Repeat til sign change.  if a sign change has
           occurred the interval is bisected and then checked again
           for a sign change which determines in which interval the
           zero lies in.  If there is no sign change between poly(xm)
           and poly(xl) set interval between xm and xr else set
           interval between xl and xr and repeat till root is located
           within the specified limits  */

	    if(((psumr*psuml)<0.0) || (psumr == 0.0)){
		roots++;

		psumm=psuml;
		for(k=0;k<=nb;k++){
		    xm = (xl+xr)/2;        	/* bisect the interval 	*/
		    psumm=cheb_poly_eva(pt,xm,order);
		    if(psumm*psuml>0.){
			psuml=psumm;
			xl=xm;
		    }
		    else{
			psumr=psumm;
			xr=xm;
		    }
		}

	       /* once zero is found, reset initial interval to xr 	*/
	       freq[j] = (xm);
	       xl = xm;
	       flag = 0;       		/* reset flag for next search 	*/
	    }
	    else{
		psuml=temp_psumr;
		xl=temp_xr;
	    }
	}
    }

    /* convert from x domain to radians */

    for(i=0; i<order; i++) {
	freq[i] = acosf(freq[i]);
    }

    return(roots);
}
Esempio n. 19
0
float              P3DMath::ACosf     (float               a)
 {
  return(acosf(a));
 }
Esempio n. 20
0
float vsACos(float theta)
{
	return acosf(theta);
}
Esempio n. 21
0
void GeometryGenerator::CreateGeosphere(float radius, UINT numSubdivisions, Mesh& mesh)
{
	// Put a cap on the number of subdivisions.
	numSubdivisions = MathHelper::Min(numSubdivisions, 5u);

	// Approximate a sphere by tessellating an icosahedron.

	const float X = 0.525731f; 
	const float Z = 0.850651f;

	Vector3 pos[12] = 
	{
		Vector3(-X, 0.0f, Z),  Vector3(X, 0.0f, Z),  
		Vector3(-X, 0.0f, -Z), Vector3(X, 0.0f, -Z),    
		Vector3(0.0f, Z, X),   Vector3(0.0f, Z, -X), 
		Vector3(0.0f, -Z, X),  Vector3(0.0f, -Z, -X),    
		Vector3(Z, X, 0.0f),   Vector3(-Z, X, 0.0f), 
		Vector3(Z, -X, 0.0f),  Vector3(-Z, -X, 0.0f)
	};

	DWORD k[60] = 
	{
		1,4,0,  4,9,0,  4,5,9,  8,5,4,  1,8,4,    
		1,10,8, 10,3,8, 8,3,5,  3,2,5,  3,7,2,    
		3,10,7, 10,6,7, 6,11,7, 6,0,11, 6,1,0, 
		10,1,6, 11,0,9, 2,11,9, 5,2,9,  11,2,7 
	};

	mesh.Vertices.resize(12);
	mesh.Indices.resize(60);

	for(UINT i = 0; i < 12; ++i)
		mesh.Vertices[i].Position = pos[i];

	for(UINT i = 0; i < 60; ++i)
		mesh.Indices[i] = k[i];

	for(UINT i = 0; i < numSubdivisions; ++i)
		Subdivide(mesh);

	// Project vertices onto sphere and scale.
	for(UINT i = 0; i < mesh.Vertices.size(); ++i)
	{
		// Project onto unit sphere.
		XMVECTOR n = XMVector3Normalize(XMLoadFloat3(&mesh.Vertices[i].Position));

		// Project onto sphere.
		XMVECTOR p = radius*n;

		XMStoreFloat3(&mesh.Vertices[i].Position, p);
		XMStoreFloat3(&mesh.Vertices[i].Normal, n);

		// Derive texture coordinates from spherical coordinates.
		float theta = MathHelper::AngleFromXY(
			mesh.Vertices[i].Position.x, 
			mesh.Vertices[i].Position.z);

		float phi = acosf(mesh.Vertices[i].Position.y / radius);

		mesh.Vertices[i].TexCoord.x = theta/XM_2PI;
		mesh.Vertices[i].TexCoord.y = phi/XM_PI;

		// Partial derivative of P with respect to theta
		mesh.Vertices[i].TangentU.x = -radius*sinf(phi)*sinf(theta);
		mesh.Vertices[i].TangentU.y = 0.0f;
		mesh.Vertices[i].TangentU.z = +radius*sinf(phi)*cosf(theta);

		XMVECTOR T = XMLoadFloat3(&mesh.Vertices[i].TangentU);
		XMStoreFloat3(&mesh.Vertices[i].TangentU, XMVector3Normalize(T));
	}
}
Esempio n. 22
0
Angle Angle::arcCosine(float x) {
	Angle a;
	a.setRadians(acosf(x));
	return a;
}
Esempio n. 23
0
static void display()
{
	if (!msg_count)
		return;
	// now decode whatever was read
	for (int di = 0; di < msg_count; di++) {
		const char * src = display_mode[di];
		if (!src)
			continue;
		printf("%02x: ", msg[di].addr);

		uint8_t * buf = msg[di].buf;

		int count = 0;
		while (*src) {
			switch (*src) {
				case '0' ... '9' :
					count = (count*10) + (*src++ - '0');
					break;
				case 'b':
					count = count ? count : 1;
					for (int bi = 0; bi < count; bi++)
						printf("%02x", *buf++);
					printf(" ");
					count = 0;
					src++;
					break;
				case 's':
					count = count ? count : 1;
					for (int bi = 0; bi < count; bi++, buf += 2) {
						short s = (buf[0] << 8) | (buf[1]);
						printf("%02x%02x %6d ", buf[0],  buf[1], s);
					}
					count = 0;
					src++;
					break;
				case 'l':
					count = count ? count : 1;
					for (int bi = 0; bi < count; bi++, buf += 4)
						printf("%02x%02x%02x%02x %8d ", buf[0], buf[1],buf[2],buf[3],
								(int)(buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | (buf[3]));
					count = 0;
					src++;
					break;
				case 't': {
					short s = (buf[0] << 8) | (buf[1]);
					printf("%02x%02x %6d %.2f", buf[0],  buf[1], s, (float)s / 256.0f);
					src++;
				}	break;
				case 'g': {
					uint16_t y = buf[0] | ((buf[3] & ~3) << 6);
					uint16_t p = buf[1] | ((buf[4] & ~3) << 6);
					uint16_t r = buf[2] | ((buf[5] & ~3) << 6);
					printf(" %c Yaw %6d %c Pitch %6d %c Roll %6d ",
							buf[3] & 2 ? '>' : ' ', y,
							buf[3] & 1 ? '>' : ' ', p,
							buf[4] & 2 ? '>' : ' ', r);
					src++;
				}	break;
				case 'c': {
					int16_t x = (buf[2] | (buf[1] << 8)) - 2048;
					int16_t y = (buf[4] | (buf[3] << 8)) - 2048;
					float deg = 0.0f;
					printf("Compass x %5d y %5d ", x, y);
					if (y) {
						float xy = (float)x / (float)y;
						if (y > 0)
							deg = 90 - (atan(xy) * 180.0f / 3.14159f);
						else
							deg = 270 - (atan(xy) * 180.0f / 3.14159f);
					} else {
						if (x < 0)
							deg = 180.0f;
					}
					printf("compass = %.2f ", deg);
					src++;
				}	break;
				case 'a': {
					int16_t x = buf[1] | (buf[0] << 8);
					int16_t y = buf[3] | (buf[2] << 8);
					int16_t z = buf[5] | (buf[4] << 8);

					float xf = (float)x / 1024.0f;
					float yf = (float)y / 1024.0f;
					float zf = (float)z / 1024.0f;

					float r = sqrtf(powf(xf, 2) + powf(yf, 2) + powf(zf, 2));

					float ax = acosf(xf / r) * (180.0f / 3.1415926535f);
					float ay = acosf(yf / r) * (180.0f / 3.1415926535f);
					float az = acosf(zf / r) * (180.0f / 3.1415926535f);
					printf("(%d %d %d) r=%.4f Accel x %3.4f y %3.4f z %3.4f ", x,y,z, r, ax, ay, az);
					src++;
				}	break;
				case ' ':
					break;
			}
		}
		printf("\n");
	}
	msg_count = 0;
}
Esempio n. 24
0
void Ini2(void)
{
	//水果数量
	Fruit_NUM = 0;
	//水果产生时间间隔
	TimeTot = 0;
	//临时定义狗运动时间
	TimeTot1 = 0;
	//捡到的石头数量
	static int StoneCount = 10;

	// 对象1的初始位置
	obj1X = -10.0f;
	obj1Y = 0.0f;
	memset(sGameObjList, 0, sizeof(GameObj)*GAME_OBJ_NUM_MAX);
	GameObj* pObj;
	int i;

	BossBlood = 2;      //初始化BOSS血量

	// 为开始画对象做准备
	AEGfxSetBackgroundColor(0.0f, 0.0f, 0.0f);
	AEGfxSetBlendMode(AE_GFX_BM_BLEND);

	// 对象实例化:游戏开始只有小盗和狗和农场主需要实例化
	// 小盗对象实例化
	Burglar = gameObjCreate(TYPE_BURGLAR, BURGLAR_SIZE, 0, 0, 0.0f);
	AE_ASSERT(Burglar);
	Burglar->posCurr.x = AEGfxGetWinMaxX()-20;
	Burglar->posCurr.y = 100.0f;
	Burglar->dirCurr = acosf(Burglar->posCurr.x / ((float)sqrt(Burglar->posCurr.x*Burglar->posCurr.x + Burglar->posCurr.y * Burglar->posCurr.y))) - PI;
	Burglar->scale = 10.0f;

	//初始化静止的石头
	for (i = 0; i < 3; i++)
	{
		pObj = gameObjCreate(TYPE_STONE_STATIC, 3.0f, 0, 0, 0.0f);
		AE_ASSERT(pObj);
		// 实例化
		// 初始化: 坐标位置 朝向和尺寸大小
		switch (i)
		{
		case 0:
			pObj->posCurr.x = 100.0f;
			pObj->posCurr.y = 100.0f;
			break;
		case 1:
			pObj->posCurr.x = 100.0f;
			pObj->posCurr.y = AEGfxGetWinMaxY() - 30;
			break;
		case 2:
			pObj->posCurr.x = AEGfxGetWinMinX() + 40;
			pObj->posCurr.y = 50.0f;
			break;
		}
		pObj->dirCurr = acosf(pObj->posCurr.x / ((float)sqrt(pObj->posCurr.x*pObj->posCurr.x + pObj->posCurr.y * pObj->posCurr.y))) - PI;

		pObj->scale = 5.0f;
	}

	//初始化看不见的陷阱
	for (int a = 0; a < 3; a++)
	{
		pObj = gameObjCreate(TYPE_TRAP_IN, 10.0f, 0, 0, 0.0f);
		AE_ASSERT(pObj);
		//初始化陷阱位置及朝向比例

		pObj->posCurr.x = a*100.0f;
		pObj->posCurr.y = -100.0f;
		pObj->dirCurr = acosf(pObj->posCurr.x / ((float)sqrt(pObj->posCurr.x*pObj->posCurr.x + pObj->posCurr.y * pObj->posCurr.y))) - PI;

		pObj->scale = 10.0f;
	}

	//初始化农场主
	pBoss = gameObjCreate(TYPE_BOSS, 10.0f, 0, 0, 0.0f);
	AE_ASSERT(pBoss);
	//初始化农场主位置及朝向比例
	pBoss->posCurr.x = 100.0f;
	pBoss->posCurr.y = 100;
	pBoss->dirCurr = acosf(pBoss->posCurr.x / ((float)sqrt(pBoss->posCurr.x*pBoss->posCurr.x + pBoss->posCurr.y * pBoss->posCurr.y))) - PI;
	pBoss->scale = 10.0f;

	//狗对象实例化 并 初始化
	for (i = 0; i < DOG_NUM; i++)
	{
		// 实例化
		pObj = gameObjCreate(TYPE_DOG, 10.0f, 0, 0, 0.0f);
		pDog[i] = pObj;//给狗分配指针
		AE_ASSERT(pObj);
		pObj->posCurr.x = 100.0f;
		pObj->posCurr.y = 100.0f;
		
		pObj->dirCurr = acosf(pObj->posCurr.x / ((float)sqrt(pObj->posCurr.x*pObj->posCurr.x + pObj->posCurr.y * pObj->posCurr.y))) - PI;

		pObj->scale = 10.0f;
	}


	//农场主血量初始化
	pObj = gameObjCreate(TYPE_BOSSBLOOD, 10.0f, 0, 0, 0.0f);
	AE_ASSERT(pObj);
	//初始化血量位置
	pObj->posCurr.x = pBoss->posCurr.x;
	pObj->posCurr.y = pBoss->posCurr.y + 35.0f;

	//主角血量初始化
	pObj = gameObjCreate(TYPE_BURGLARBLOOD, 10.0f, 0, 0, 0.0f);
	AE_ASSERT(pObj);
	//初始化血量位置
	pObj->posCurr.x = Burglar->posCurr.x;
	pObj->posCurr.y = Burglar->posCurr.y + 35.0f;

	//地图的引入
	FILE *fp = NULL;
	if ((fp = fopen("wdp.txt", "r")) != NULL)
	{
		int length = 0, width = 0;
		fscanf(fp, "%d%d", &length, &width);
		int i = 0, j = 0;
		for (; i < length; i++)
		{
			for (j = 0; j < width; j++)
			{
				fscanf(fp, "%d", &mapinfo[i][j]);
			}
		}

		//读入地图
		for (i = 0; i < length; i++)
		{
			for (j = 0; j < width; j++)
			{
				if (mapinfo[i][j] == 1)
				{
					//画地图
					pObj = gameObjCreate(TYPE_MAP, 1.0f, 0, 0, 0.0f);
					AE_ASSERT(pObj);
					pObj->posCurr.y = i * 40 - 400.0f;
					pObj->posCurr.x = j*15 - 300.0f;
				}
			}
		}
	}
	else if ((fp = fopen("wdp.txt", "r")) == NULL)
	{
		KeyPressed[KeyESC] = TRUE;
	}

	// 签到
	fprintf(fp, "Level2:Initialize\n");
}
Esempio n. 25
0
float ArrowEffects::GetXPos( const PlayerState* pPlayerState, int iColNum, float fYOffset ) 
{
	float fPixelOffsetFromCenter = 0; // fill this in below

	const Style* pStyle = GAMESTATE->GetCurrentStyle(pPlayerState->m_PlayerNumber);
	const float* fEffects = pPlayerState->m_PlayerOptions.GetCurrent().m_fEffects;

	// TODO: Don't index by PlayerNumber.
	const Style::ColumnInfo* pCols = pStyle->m_ColumnInfo[pPlayerState->m_PlayerNumber];
	PerPlayerData &data = g_EffectData[pPlayerState->m_PlayerNumber];

	if( fEffects[PlayerOptions::EFFECT_TORNADO] != 0 )
	{
		const float fRealPixelOffset = pCols[iColNum].fXOffset * pPlayerState->m_NotefieldZoom;
		const float fPositionBetween = SCALE( fRealPixelOffset, data.m_fMinTornadoX[iColNum], data.m_fMaxTornadoX[iColNum], 
						     TORNADO_POSITION_SCALE_TO_LOW, TORNADO_POSITION_SCALE_TO_HIGH );
		float fRads = acosf( fPositionBetween );
		fRads += fYOffset * TORNADO_OFFSET_FREQUENCY / SCREEN_HEIGHT;

		const float fAdjustedPixelOffset = SCALE( RageFastCos(fRads), TORNADO_OFFSET_SCALE_FROM_LOW, TORNADO_OFFSET_SCALE_FROM_HIGH, 
							 data.m_fMinTornadoX[iColNum], data.m_fMaxTornadoX[iColNum] );

		fPixelOffsetFromCenter += (fAdjustedPixelOffset - fRealPixelOffset) * fEffects[PlayerOptions::EFFECT_TORNADO];
	}

	if( fEffects[PlayerOptions::EFFECT_DRUNK] != 0 )
		fPixelOffsetFromCenter += fEffects[PlayerOptions::EFFECT_DRUNK] * 
			( RageFastCos( RageTimer::GetTimeSinceStartFast() + iColNum*DRUNK_COLUMN_FREQUENCY
				      + fYOffset*DRUNK_OFFSET_FREQUENCY/SCREEN_HEIGHT) * ARROW_SIZE*DRUNK_ARROW_MAGNITUDE );
	if( fEffects[PlayerOptions::EFFECT_FLIP] != 0 )
	{
		const int iFirstCol = 0;
		const int iLastCol = pStyle->m_iColsPerPlayer-1;
		const int iNewCol = SCALE( iColNum, iFirstCol, iLastCol, iLastCol, iFirstCol );
		const float fOldPixelOffset = pCols[iColNum].fXOffset * pPlayerState->m_NotefieldZoom;
		const float fNewPixelOffset = pCols[iNewCol].fXOffset * pPlayerState->m_NotefieldZoom;
		const float fDistance = fNewPixelOffset - fOldPixelOffset;
		fPixelOffsetFromCenter += fDistance * fEffects[PlayerOptions::EFFECT_FLIP];
	}
	if( fEffects[PlayerOptions::EFFECT_INVERT] != 0 )
		fPixelOffsetFromCenter += data.m_fInvertDistance[iColNum] * fEffects[PlayerOptions::EFFECT_INVERT];

	if( fEffects[PlayerOptions::EFFECT_BEAT] != 0 )
	{
		const float fShift = data.m_fBeatFactor*RageFastSin( fYOffset / BEAT_OFFSET_HEIGHT + PI/BEAT_PI_HEIGHT );
		fPixelOffsetFromCenter += fEffects[PlayerOptions::EFFECT_BEAT] * fShift;
	}

	if( fEffects[PlayerOptions::EFFECT_XMODE] != 0 )
	{
		// based off of code by v1toko for StepNXA, except it should work on
		// any gametype now.
		switch( pStyle->m_StyleType )
		{
			case StyleType_OnePlayerTwoSides:
			case StyleType_TwoPlayersSharedSides: // fall through?
				{
					// find the middle, and split based on iColNum
					// it's unknown if this will work for routine.
					const int iMiddleColumn = static_cast<int>(floor(pStyle->m_iColsPerPlayer/2.0f));
					if( iColNum > iMiddleColumn-1 )
						fPixelOffsetFromCenter += fEffects[PlayerOptions::EFFECT_XMODE]*-(fYOffset);
					else
						fPixelOffsetFromCenter += fEffects[PlayerOptions::EFFECT_XMODE]*fYOffset;
				}
				break;
			case StyleType_OnePlayerOneSide:
			case StyleType_TwoPlayersTwoSides: // fall through
				{
					// the code was the same for both of these cases in StepNXA.
					if( pPlayerState->m_PlayerNumber == PLAYER_2 )
						fPixelOffsetFromCenter += fEffects[PlayerOptions::EFFECT_XMODE]*-(fYOffset);
					else
						fPixelOffsetFromCenter += fEffects[PlayerOptions::EFFECT_XMODE]*fYOffset;
				}
				break;
			DEFAULT_FAIL(pStyle->m_StyleType);
		}
	}

	fPixelOffsetFromCenter += pCols[iColNum].fXOffset * pPlayerState->m_NotefieldZoom;

	if( fEffects[PlayerOptions::EFFECT_TINY] != 0 )
	{
		// Allow Tiny to pull tracks together, but not to push them apart.
		float fTinyPercent = fEffects[PlayerOptions::EFFECT_TINY];
		fTinyPercent = min( powf(TINY_PERCENT_BASE, fTinyPercent), (float)TINY_PERCENT_GATE );
		fPixelOffsetFromCenter *= fTinyPercent;
	}

	return fPixelOffsetFromCenter;
}
Esempio n. 26
0
float Vector3DEx::angle(Vector3DEx& other)
{
	float cosAngle = (*this*other)/(length()*other.length());
	float ang = acosf(cosAngle);
	return ang;
}
Esempio n. 27
0
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
    size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
  // The RF is formed as this x_axis | y_axis | normal
  Eigen::Map<Eigen::Vector3f> x_axis (rf);
  Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
  Eigen::Map<Eigen::Vector3f> normal (rf + 6);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  if (neighb_cnt == 0)
  {
    for (size_t i = 0; i < desc.size (); ++i)
      desc[i] = std::numeric_limits<float>::quiet_NaN ();

    memset (rf, 0, sizeof (rf[0]) * 9);
    return (false);
  }

  float minDist = std::numeric_limits<float>::max ();
  int minIndex = -1;
  for (size_t i = 0; i < nn_indices.size (); i++)
  {
	  if (nn_dists[i] < minDist)
	  {
      minDist = nn_dists[i];
      minIndex = nn_indices[i];
	  }
  }
  
  // Get origin point
  Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
  // Get origin normal
  // Use pre-computed normals
  normal = normals[minIndex].getNormalVector3fMap ();

  // Compute and store the RF direction
  x_axis[0] = static_cast<float> (rnd ());
  x_axis[1] = static_cast<float> (rnd ());
  x_axis[2] = static_cast<float> (rnd ());
  if (!pcl::utils::equal (normal[2], 0.0f))
    x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
  else if (!pcl::utils::equal (normal[1], 0.0f))
    x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
  else if (!pcl::utils::equal (normal[0], 0.0f))
    x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];

  x_axis.normalize ();

  // Check if the computed x axis is orthogonal to the normal
  assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));

  // Store the 3rd frame vector
  y_axis = normal.cross (x_axis);

  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal (nn_dists[ne], 0.0f))
		  continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    /// ----- Compute current neighbour polar coordinates -----
    /// Get distance between the neighbour and the origin
    float r = sqrt (nn_dists[ne]); 
    
    /// Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    /// Normalize to compute the dot product
    proj.normalize ();
    
    /// Compute the angle between the projection and the x axis in the interval [0,360] 
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));

    // Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    // Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_+1; rad++) 
    {
      if (r <= radii_interval_[rad]) 
      {
        j = rad-1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_+1; ang++) 
    {
      if (theta <= theta_divisions_[ang]) 
      {
        k = ang-1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_+1; ang++) 
    {
      if (phi <= phi_divisions_[ang]) 
      {
        l = ang-1;
        break;
      }
    }

    // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_distances;
    int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
    // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
    if (point_density == 0)
      continue;

    float w = (1.0f / static_cast<float> (point_density)) * 
              volume_lut_[(l*elevation_bins_*radius_bins_) +  (k*radius_bins_) + j];
      
    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (w != w)
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondant Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour

  // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user 
  memset (rf, 0, sizeof (rf[0]) * 9);
  return (true);
}
Esempio n. 28
0
//  Helper function returns the positive angle (in radians) between two 3D vectors
float angleBetween(const glm::vec3& v1, const glm::vec3& v2) {
    return acosf((glm::dot(v1, v2)) / (glm::length(v1) * glm::length(v2)));
}
Esempio n. 29
0
bool Compass::consistent() const
{
    Vector3f primary_mag_field = get_field();
    Vector3f primary_mag_field_norm;

    if (!primary_mag_field.is_zero()) {
        primary_mag_field_norm = primary_mag_field.normalized();
    } else {
        return false;
    }

    Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y);
    Vector2f primary_mag_field_xy_norm;

    if (!primary_mag_field_xy.is_zero()) {
        primary_mag_field_xy_norm = primary_mag_field_xy.normalized();
    } else {
        return false;
    }

    for (uint8_t i=0; i<get_count(); i++) {
        if (use_for_yaw(i)) {
            Vector3f mag_field = get_field(i);
            Vector3f mag_field_norm;

            if (!mag_field.is_zero()) {
                mag_field_norm = mag_field.normalized();
            } else {
                return false;
            }

            Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y);
            Vector2f mag_field_xy_norm;

            if (!mag_field_xy.is_zero()) {
                mag_field_xy_norm = mag_field_xy.normalized();
            } else {
                return false;
            }

            float xyz_ang_diff = acosf(constrain_float(mag_field_norm * primary_mag_field_norm,-1.0f,1.0f));
            float xy_ang_diff  = acosf(constrain_float(mag_field_xy_norm*primary_mag_field_xy_norm,-1.0f,1.0f));
            float xy_len_diff  = (primary_mag_field_xy-mag_field_xy).length();

            // check for gross misalignment on all axes
            bool xyz_ang_diff_large = xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF;

            // check for an unacceptable angle difference on the xy plane
            bool xy_ang_diff_large = xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF;

            // check for an unacceptable length difference on the xy plane
            bool xy_length_diff_large = xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF;

            // check for inconsistency in the XY plane
            if (xyz_ang_diff_large || xy_ang_diff_large || xy_length_diff_large) {
                return false;
            }
        }
    }
    return true;
}
Esempio n. 30
0
void Functions::acos(Aurora::NWScript::FunctionContext &ctx) {
	ctx.getReturn() = Common::rad2deg(acosf(ctx.getParams()[0].getFloat()));
}