//! 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); }
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 §ion = 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 §ion = 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;*/ }
float __racosd(float x) { return (acosf(x) * 57.29577951308232F); }
// カプセル描画 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); }
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 }
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; }
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; } } }
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; }
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))) ; } } }
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; }
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)); }
float acos( float a ){ ASSERT( a >= -1.f ); ASSERT( a <= 1.f ); return acosf( a ) * TO_DEG; }
//----------------------------------------------------------------------------- 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; }
// only defined for 0<=t<=1 static float console_scale_func(float t) { //return t; return sinf(acosf(1.0f-t)); }
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 }
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); }
float P3DMath::ACosf (float a) { return(acosf(a)); }
float vsACos(float theta) { return acosf(theta); }
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)); } }
Angle Angle::arcCosine(float x) { Angle a; a.setRadians(acosf(x)); return a; }
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; }
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"); }
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; }
float Vector3DEx::angle(Vector3DEx& other) { float cosAngle = (*this*other)/(length()*other.length()); float ang = acosf(cosAngle); return ang; }
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); }
// 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))); }
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; }
void Functions::acos(Aurora::NWScript::FunctionContext &ctx) { ctx.getReturn() = Common::rad2deg(acosf(ctx.getParams()[0].getFloat())); }