void SetRotateMatrix(float* m, vertex p1, vertex p2, float radians) { float R[16], T[16]; SetIdentityMatrix(R); float length=sqrt( (p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)+ (p2.z-p1.z)*(p2.z-p1.z) ); float cosa=cos(radians/2.0); float sina=sin(radians/2.0); float a=sina*(p2.x-p1.x)/length; float b=sina*(p2.y-p1.y)/length; float c=sina*(p2.z-p1.z)/length; SetTranslateMatrix2(m, -p1.x, -p1.y, -p1.z); SetTranslateMatrix2(T, p1.x, p1.y, p1.z); SetIdentityMatrix(R); R[0]=1.0-2*b*b-2*c*c; R[1]=2*a*b-2*cosa*c; R[2]=2*a*c+2*cosa*b; R[4]=2*a*b+2*cosa*c; R[5]=1.0-2*a*a-2*c*c; R[6]=2*b*c-2*cosa*a; R[8]=2*a*c-2*cosa*b; R[9]=2*b*c+2*cosa*a; R[10]=1.0-2*a*a-2*b*b; Multiply44(R, m); Multiply44(T, m); }
void SetOriginRotateMatrix(float* m, vertex radians) { float fRx[16], fRy[16], fRz[16]; SetIdentityMatrix(fRx); SetIdentityMatrix(fRy); SetIdentityMatrix(fRz); SetIdentityMatrix(m); fRx[0]=1; fRx[5]=cos(radians.x); fRx[6]=-1*sin(radians.x); fRx[9]=-1*fRx[6]; fRx[10]=fRx[5]; fRx[15]=1; // fRy[0]=cos(radians.y); fRy[2]=sin(radians.y); fRy[5]=1; fRy[8]=-1*fRy[2]; fRy[10]=fRy[0]; fRy[15]=1; // fRz[0]=cos(radians.z); fRz[1]=-1*sin(radians.z); fRz[4]=-1*fRz[1]; fRz[5]=fRz[0]; fRz[10]=1; fRz[15]=1; // Must follow z/x/y order due to phantom handle design; Multiply44(fRz, m); Multiply44(fRx, m); Multiply44(fRy, m); }
// add Properties->Debugging ->environment PATH = %PATH%; D:\VTK_bin\bin\Debug DepthSensor::DepthSensor() : mNuiSensor(NULL) , mNextDepthFrameEvent(INVALID_HANDLE_VALUE) , mDepthStreamHandle(INVALID_HANDLE_VALUE) , mDrawDepth(NULL) , mdepthImageResolution(NUI_IMAGE_RESOLUTION_640x480) , m_pVolume(NULL) , cDepthImagePixels(0) , m_pDepthImagePixelBuffer(NULL) , m_pDepthFloatImage(NULL) , m_pPointCloud(NULL) , m_pShadedSurface(NULL) , m_bMirrorDepthFrame(false) , m_bTranslateResetPoseByMinDepthThreshold(true) , m_cLostFrameCounter(0) , m_bTrackingFailed(false) , m_bAutoResetReconstructionWhenLost(false) , m_bAutoResetReconstructionOnTimeout(true) , m_fStartTime(0) , m_isInit(false) , m_saveMeshFormat(Stl) , filename() { // Get the depth frame size from the NUI_IMAGE_RESOLUTION enum DWORD WIDTH = 0, HEIGHT = 0; NuiImageResolutionToSize(mdepthImageResolution, WIDTH, HEIGHT); cDepthWidth = WIDTH; cDepthHeight = HEIGHT; cDepthImagePixels = cDepthWidth*cDepthHeight; //create heap storage for depth pixel data in RGBX format m_depthRGBX = new BYTE[cDepthWidth*cDepthHeight*cBytesPerPixel]; // Define a cubic Kinect Fusion reconstruction volume, // with the Kinect at the center of the front face and the volume directly in front of Kinect. reconstructionParams.voxelsPerMeter = 256; // 1000mm / 256vpm = ~3.9mm/voxel reconstructionParams.voxelCountX = 512; // 512 / 256vpm = 2m wide reconstruction reconstructionParams.voxelCountY = 384; // Memory = 512*384*512 * 4bytes per voxel reconstructionParams.voxelCountZ = 512; // Require 512MB GPU memory // These parameters are for optionally clipping the input depth image m_fMinDepthThreshold = NUI_FUSION_DEFAULT_MINIMUM_DEPTH; // min depth in meters m_fMaxDepthThreshold = NUI_FUSION_DEFAULT_MAXIMUM_DEPTH; // max depth in meters // This parameter is the temporal averaging parameter for depth integration into the reconstruction m_cMaxIntegrationWeight = NUI_FUSION_DEFAULT_INTEGRATION_WEIGHT; // Reasonable for static scenes SetIdentityMatrix(m_worldToCameraTransform); SetIdentityMatrix(m_defaultWorldToVolumeTransform); m_cLastDepthFrameTimeStamp.QuadPart = 0; // Initialize synchronization objects InitializeCriticalSection(&m_lockVolume); }
void SetTranslateMatrix(float* m, vertex t) { SetIdentityMatrix(m); m[3]=t.x; m[7]=t.y; m[11]=t.z; }
void SetTranslateMatrix2(float* m, float x, float y, float z) { SetIdentityMatrix(m); m[3]=x; m[7]=y; m[11]=z; }
void SetScaleMatrix(float* m, float x, float y, float z) { SetIdentityMatrix(m); m[0]=x; m[5]=y; m[10]=z; }
void Device_OpenGL::Draw2DTextureFullViewport(Texture* texture) { DisableDepthTest(); PushMatrix(); SetIdentityMatrix(); UnbindVBO(VBO::VBO_TARGET_ARRAY_BUFFER); //------------------------------------------- float x = Screen::GetInstance()->GetRatio(); float y = 1.0f; float z = -1.0f; float vertices[30] = { -x, y, z, 0.0f, 1.0f, -x, -y, z, 0.0f, 0.0f, x, -y, z, 1.0f, 0.0f, -x, y, z, 0.0f, 1.0f, x, -y, z, 1.0f, 0.0f, x, y, z, 1.0f, 1.0f }; SetAttributePointer(ATTRIBUTE_FLOAT3_POSITION, vertices, sizeof(float) * 5); SetAttributePointer(ATTRIBUTE_FLOAT2_TEXCOORD_DIFFUSE, vertices + 3, sizeof(float) * 5); Device_Base::GetInstance()->UpdateVertexAttribPointer(); Device_Base::GetInstance()->SetUniformTexture(UNIFORM_TEXTURE_DIFFUSE, texture); Device_Base::GetInstance()->SetUniformMatrix(UNIFORM_MATRIX_MODEL, Device_Base::GetInstance()->GetMatrixWorld()); Device_Base::GetInstance()->SetUniformMatrix(UNIFORM_MATRIX_PROJECTION, m_MatrixProjection2DLogical); Device_Base::GetInstance()->DrawArray(Device_Base::PRIMITIVE_TRIANGLE_LIST, 0, 6); //------------------------------------------- EnableDepthTest(); PopMatrix(); }
static void RagdollAddConstraints( IPhysicsEnvironment *pPhysEnv, ragdoll_t &ragdoll, const ragdollparams_t ¶ms, const cache_ragdollconstraint_t *pConstraints, int constraintCount ) { constraint_ragdollparams_t constraint; for ( int i = 0; i < constraintCount; i++ ) { constraint.Defaults(); V_memcpy( constraint.axes, pConstraints[i].axes, sizeof(constraint.axes) ); if ( params.jointFrictionScale > 0 ) { for ( int k = 0; k < 3; k++ ) { constraint.axes[k].torque *= params.jointFrictionScale; } } int parentIndex = pConstraints[i].parentIndex; int childIndex = pConstraints[i].childIndex; constraint.childIndex = childIndex; constraint.parentIndex = parentIndex; constraint.useClockwiseRotations = true; constraint.constraintToAttached = pConstraints[i].constraintToAttached; // UNDONE: We could transform the constraint limit axes relative to the bone space // using this data. Do we need that feature? SetIdentityMatrix( constraint.constraintToReference ); ragdoll.list[childIndex].pConstraint = pPhysEnv->CreateRagdollConstraint( ragdoll.list[childIndex].pObject, ragdoll.list[parentIndex].pObject, ragdoll.pGroup, constraint ); } }
const matrix3x4_t & C_EnvelopeFX::RenderableToWorldTransform() { static matrix3x4_t mat; SetIdentityMatrix( mat ); PositionMatrix( GetRenderOrigin(), mat ); return mat; }
const matrix3x4_t & CParticleEffectBinding::RenderableToWorldTransform() { static matrix3x4_t mat; SetIdentityMatrix( mat ); PositionMatrix( GetRenderOrigin(), mat ); return mat; }
OSErr QTWired_AddWraparoundMatrixOnIdle (QTAtomContainer theContainer) { MatrixRecord myMinMatrix, myMaxMatrix, myDeltaMatrix; long myFlags = kActionFlagActionIsDelta | kActionFlagParameterWrapsAround; QTAtom myActionAtom; OSErr myErr = noErr; myMinMatrix.matrix[0][0] = myMinMatrix.matrix[0][1] = myMinMatrix.matrix[0][2] = EndianS32_NtoB(0xffffffff); myMinMatrix.matrix[1][0] = myMinMatrix.matrix[1][1] = myMinMatrix.matrix[1][2] = EndianS32_NtoB(0xffffffff); myMinMatrix.matrix[2][0] = myMinMatrix.matrix[2][1] = myMinMatrix.matrix[2][2] = EndianS32_NtoB(0xffffffff); myMaxMatrix.matrix[0][0] = myMaxMatrix.matrix[0][1] = myMaxMatrix.matrix[0][2] = EndianS32_NtoB(0x7fffffff); myMaxMatrix.matrix[1][0] = myMaxMatrix.matrix[1][1] = myMaxMatrix.matrix[1][2] = EndianS32_NtoB(0x7fffffff); myMaxMatrix.matrix[2][0] = myMaxMatrix.matrix[2][1] = myMaxMatrix.matrix[2][2] = EndianS32_NtoB(0x7fffffff); myMinMatrix.matrix[2][1] = EndianS32_NtoB(Long2Fix((1 * kSpriteTrackHeight / 4) - (kPenguinHeight / 2))); myMaxMatrix.matrix[2][1] = EndianS32_NtoB(Long2Fix((3 * kSpriteTrackHeight / 4) - (kPenguinHeight / 2))); SetIdentityMatrix(&myDeltaMatrix); myDeltaMatrix.matrix[2][1] = Long2Fix(1); // change location myErr = WiredUtils_AddSpriteSetMatrixAction(theContainer, kParentAtomIsContainer, kQTEventIdle, 0, NULL, 0, 0, NULL, &myDeltaMatrix, &myActionAtom); if (myErr != noErr) goto bail; myErr = WiredUtils_AddActionParameterOptions(theContainer, myActionAtom, 1, myFlags, sizeof(myMinMatrix), &myMinMatrix, sizeof(myMaxMatrix), &myMaxMatrix); bail: return(myErr); }
void initializeFusion() { // 再構成パラメーターの設定 reconstructionParameters.voxelsPerMeter = 256; reconstructionParameters.voxelCountX = 512; reconstructionParameters.voxelCountY = 384; reconstructionParameters.voxelCountZ = 512; // Reconstructionの作成 SetIdentityMatrix( worldToCameraTransform ); ERROR_CHECK( NuiFusionCreateColorReconstruction( &reconstructionParameters, NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE::NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_AMP, -1, &worldToCameraTransform, &reconstruction ) ); // カメラパラメーターの設定 cameraParameters.focalLengthX = NUI_KINECT_DEPTH_NORM_FOCAL_LENGTH_X; cameraParameters.focalLengthY = NUI_KINECT_DEPTH_NORM_FOCAL_LENGTH_Y; cameraParameters.principalPointX = NUI_KINECT_DEPTH_NORM_PRINCIPAL_POINT_X; cameraParameters.principalPointY = NUI_KINECT_DEPTH_NORM_PRINCIPAL_POINT_Y; // Image Frameの作成 ERROR_CHECK( NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE::NUI_FUSION_IMAGE_TYPE_FLOAT, depthWidth, depthHeight, &cameraParameters, &depthImageFrame ) ); ERROR_CHECK( NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE::NUI_FUSION_IMAGE_TYPE_FLOAT, depthWidth, depthHeight, &cameraParameters, &smoothDepthImageFrame ) ); ERROR_CHECK( NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE::NUI_FUSION_IMAGE_TYPE_COLOR, depthWidth, depthHeight, &cameraParameters, &colorImageFrame ) ); ERROR_CHECK( NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE::NUI_FUSION_IMAGE_TYPE_POINT_CLOUD, depthWidth, depthHeight, &cameraParameters, &pointCloudImageFrame ) ); ERROR_CHECK( NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE::NUI_FUSION_IMAGE_TYPE_COLOR, depthWidth, depthHeight, &cameraParameters, &surfaceImageFrame ) ); /*ERROR_CHECK( NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE::NUI_FUSION_IMAGE_TYPE_COLOR, depthWidth, depthHeight, &cameraParameters, &normalImageFrame ) );*/ }
static void RagdollAddConstraint( IPhysicsEnvironment *pPhysEnv, ragdoll_t &ragdoll, const ragdollparams_t ¶ms, constraint_ragdollparams_t &constraint ) { if( constraint.childIndex == constraint.parentIndex ) { DevMsg( 1, "Bogus constraint on ragdoll %s\n", params.pStudioHdr->pszName() ); constraint.childIndex = -1; constraint.parentIndex = -1; } if ( constraint.childIndex >= 0 && constraint.parentIndex >= 0 ) { Assert(constraint.childIndex<ragdoll.listCount); ragdollelement_t &childElement = ragdoll.list[constraint.childIndex]; // save parent index childElement.parentIndex = constraint.parentIndex; if ( params.jointFrictionScale > 0 ) { for ( int k = 0; k < 3; k++ ) { constraint.axes[k].torque *= params.jointFrictionScale; } } // this parent/child pair is not usually a parent/child pair in the skeleton. There // are often bones in between that are collapsed for simulation. So we need to compute // the transform. Studio_CalcBoneToBoneTransform( params.pStudioHdr, ragdoll.boneIndex[constraint.childIndex], ragdoll.boneIndex[constraint.parentIndex], constraint.constraintToAttached ); MatrixGetColumn( constraint.constraintToAttached, 3, childElement.originParentSpace ); // UNDONE: We could transform the constraint limit axes relative to the bone space // using this data. Do we need that feature? SetIdentityMatrix( constraint.constraintToReference ); childElement.pConstraint = pPhysEnv->CreateRagdollConstraint( childElement.pObject, ragdoll.list[constraint.parentIndex].pObject, ragdoll.pGroup, constraint ); } }
const matrix3x4_t& CBulletManager::CBullet::RenderableToWorldTransform() { static matrix3x4_t mat; SetIdentityMatrix( mat ); PositionMatrix( GetRenderOrigin(), mat ); return mat; }
// Reset Reconstruction inline void Kinect::reset() { // Set Identity Matrix SetIdentityMatrix( worldToCameraTransform ); // Reset Reconstruction ERROR_CHECK( reconstruction->ResetReconstruction( &worldToCameraTransform, nullptr ) ); }
/* call-seq: reset_transformations() Revert any transformations (scale, translate, rotate) performed on this track. */ static VALUE track_reset_transformations(VALUE obj) { MatrixRecord matrix; GetTrackMatrix(TRACK(obj), &matrix); SetIdentityMatrix(&matrix); SetTrackMatrix(TRACK(obj), &matrix); return obj; }
//////////////////////////////////////////////////////////////////////////////// // virtual bool LLMediaImplQuickTime::setMovieBoxEnhanced( Rect* rect ) { // get movie rect GetMovieNaturalBoundsRect( mMovieHandle, rect ); int natural_width = ( rect->right - rect->left ); int natural_height = ( rect->bottom - rect->top ); int width = natural_width; int height = natural_height; // if the user has requested a specific size, use it: if ((mMediaRequestedWidth != 0) && (mMediaRequestedHeight != 0)) { width = mMediaRequestedWidth; height = mMediaRequestedHeight; } // if the user has requested, resize media to exactly fit texture if (mAutoScaled) { width = LLMediaManager::textureWidthFromMediaWidth( width ); height = LLMediaManager::textureHeightFromMediaHeight( height ); } // make sure it falls in valid range if ( width < mMinWidth ) width = mMinWidth; if ( width > mMaxWidth ) width = mMaxWidth; if ( height < mMinHeight ) height = mMinHeight; if ( height > mMaxHeight ) height = mMaxHeight; // scale movie to fit rect and invert vertically to match opengl image format MatrixRecord transform; SetIdentityMatrix( &transform ); // transforms are additive so start from identify matrix double scaleX = (double) width / natural_width; double scaleY = -1.0 * (double) height / natural_height; double centerX = width / 2.0; double centerY = height / 2.0; ScaleMatrix( &transform, X2Fix ( scaleX ), X2Fix ( scaleY ), X2Fix ( centerX ), X2Fix ( centerY ) ); SetMovieMatrix( mMovieHandle, &transform ); // return the new rect rect->right = width; rect->bottom = height; rect->left = 0; rect->top = 0; return true; }
void SetScaleMatrix(float* m, float x, float y, float z, vertex center) { SetIdentityMatrix(m); m[0]=x; m[3]=(1-x) * center.x; m[5]=y; m[7]=(1-y) * center.y; m[10]=z; m[11]=(1-z) * center.z; }
/// // Setea la matriz de transformacion de el ojo /// void SetViewTransformation(ViewPointRec *view_point_ref, Matrix view_transformation){ SetIdentityMatrix(view_transformation); view_transformation[0][0] = (-view_point_ref->sin_theta); view_transformation[0][1] = (-view_point_ref->cos_theta) * view_point_ref->cos_phi; view_transformation[0][2] = (-view_point_ref->cos_theta) * view_point_ref->sin_phi; view_transformation[1][0] = view_point_ref->cos_theta; view_transformation[1][1] = (-view_point_ref->sin_theta) * view_point_ref->cos_phi; view_transformation[1][2] = (-view_point_ref->sin_theta) * view_point_ref->sin_phi; view_transformation[2][1] = view_point_ref->sin_phi; view_transformation[2][2] = (-view_point_ref->cos_phi); view_transformation[3][2] = view_point_ref->rho; }
//----------------------------------------------------------------------------- // Purpose: // Input : entityIndex - // attachmentIndex - // &transform - //----------------------------------------------------------------------------- bool FX_GetAttachmentTransform( ClientEntityHandle_t hEntity, int attachmentIndex, matrix3x4_t &transform ) { Vector origin; QAngle angles; if ( FX_GetAttachmentTransform( hEntity, attachmentIndex, &origin, &angles ) ) { AngleMatrix( angles, origin, transform ); return true; } // Entity doesn't exist SetIdentityMatrix( transform ); return false; }
/****************************************************************** * init_board *******************************************************************/ object_gl* init_board(){ object_gl *board = calloc(1, sizeof(object_gl)); board->num_vertx = 4; board->num_vectr = 2; board->vertx_per_vectr = 3; board->rotation_spd = 0; board->rotation_dir = 1; board->alpha = 0.75; board->vertx_buffer_data = malloc(board->num_vertx * 3 * sizeof(GLfloat)); board->normal_buffer_data = malloc(board->num_vertx * board->vertx_per_vectr * sizeof(GLfloat)); board->color_buffer_data = malloc(board->num_vertx * 2 * sizeof(GLfloat)); board->index_buffer_data = malloc(board->num_vectr * board->vertx_per_vectr * sizeof(GLushort)); SetIdentityMatrix(board->model_matrix); return board; }
BOOL CoSkyMesh::Initialize(CoGeometry* pGeometry,I3DModel* pModel,DWORD dwFlag) { m_pGeometry = pGeometry; m_pModel = pModel; pModel->AddRef(); m_dwRefIndex = pModel->Reference(); SetIdentityMatrix(&m_matTransform); m_dwAlpha = 255; m_dwCurrentMotionIndex = 1; m_dwCurrentFrame = 0; return TRUE; }
/// <summary> /// Reset the reconstruction camera pose and clear the volume. /// </summary> /// <returns>S_OK on success, otherwise failure code</returns> HRESULT CKinectFusion::ResetReconstruction() { if (nullptr == m_pVolume) { return E_FAIL; } HRESULT hr = S_OK; SetIdentityMatrix(m_worldToCameraTransform); // Translate the reconstruction volume location away from the world origin by an amount equal // to the minimum depth threshold. This ensures that some depth signal falls inside the volume. // If set false, the default world origin is set to the center of the front face of the // volume, which has the effect of locating the volume directly in front of the initial camera // position with the +Z axis into the volume along the initial camera direction of view. if (m_bTranslateResetPoseByMinDepthThreshold) { Matrix4 worldToVolumeTransform = m_defaultWorldToVolumeTransform; // Translate the volume in the Z axis by the minDepthThreshold distance float minDist = (m_fMinDepthThreshold < m_fMaxDepthThreshold) ? m_fMinDepthThreshold : m_fMaxDepthThreshold; worldToVolumeTransform.M43 -= (minDist * m_reconstructionParams.voxelsPerMeter); hr = m_pVolume->ResetReconstruction(&m_worldToCameraTransform, &worldToVolumeTransform); } else { hr = m_pVolume->ResetReconstruction(&m_worldToCameraTransform, nullptr); } m_cLostFrameCounter = 0; m_cFrameCounter = 0; if (SUCCEEDED(hr)) { m_bTrackingFailed = false; std::cout << "Reconstruction has been reset." << std::endl; } else { std::cout << "Failed to reset reconstruction." << std::endl; } return hr; }
const matrix3x4_t& CCollisionProperty::CollisionToWorldTransform() const { static matrix3x4_t s_matTemp[4]; static int s_nIndex = 0; matrix3x4_t &matResult = s_matTemp[s_nIndex]; s_nIndex = (s_nIndex+1) & 0x3; if ( IsBoundsDefinedInEntitySpace() ) { return m_pOuter->EntityToWorldTransform(); } SetIdentityMatrix( matResult ); MatrixSetColumn( GetCollisionOrigin(), 3, matResult ); return matResult; }
bool C_WalkerStrider::GetAttachment( int iAttachment, matrix3x4_t &attachmentToWorld ) { // // // This is a TOTAL hack, but we don't have any nodes that work well at all for mounted guns. // // studiohdr_t *pStudioHdr = GetModelPtr( ); if ( !pStudioHdr || iAttachment < 1 || iAttachment > pStudioHdr->numattachments ) { return false; } Vector vLocalPos( 0, 0, 0 ); mstudioattachment_t *pAttachment = pStudioHdr->pAttachment( iAttachment-1 ); if ( stricmp( pAttachment->pszName(), "build_point_left_gun" ) == 0 ) { vLocalPos.y = sideDist; } else if ( stricmp( pAttachment->pszName(), "build_point_right_gun" ) == 0 ) { vLocalPos.y = -sideDist; } else if ( stricmp( pAttachment->pszName(), "ThirdPersonCameraOrigin" ) == 0 ) { } else { // Ok, it's not one of our magical attachments. Use the regular attachment setup stuff. return BaseClass::GetAttachment( iAttachment, attachmentToWorld ); } if ( m_bCrouched ) { vLocalPos.z += downDist; } // Now build the output matrix. matrix3x4_t localMatrix; SetIdentityMatrix( localMatrix ); PositionMatrix( vLocalPos, localMatrix ); ConcatTransforms( EntityToWorldTransform(), localMatrix, attachmentToWorld ); return true; }
static void RagdollCreateObjects( IPhysicsCollision *pPhysCollision, IPhysicsEnvironment *pPhysEnv, IPhysicsSurfaceProps *pSurfaceDatabase, ragdoll_t &ragdoll, const ragdollparams_t ¶ms ) { ragdoll.listCount = 0; ragdoll.pGroup = NULL; memset( ragdoll.list, 0, sizeof(ragdoll.list) ); if ( !params.pCollide || params.pCollide->solidCount > RAGDOLL_MAX_ELEMENTS ) return; IVPhysicsKeyParser *pParse = pPhysCollision->VPhysicsKeyParserCreate( params.pCollide->pKeyValues ); ragdoll.pGroup = pPhysEnv->CreateConstraintGroup(); while ( !pParse->Finished() ) { const char *pBlock = pParse->GetCurrentBlockName(); if ( !strcmpi( pBlock, "solid" ) ) { solid_t solid; // collisions off by default pParse->ParseSolid( &solid, NULL ); if ( solid.index >= 0 && solid.index < params.pCollide->solidCount) { Assert( ragdoll.listCount == solid.index ); int boneIndex = Studio_BoneIndexByName( params.pStudioHdr, solid.name ); ragdoll.boneIndex[ragdoll.listCount] = boneIndex; if ( boneIndex >= 0 ) { solid.params.rotInertiaLimit = 0.5; solid.params.pGameData = params.pGameData; int surfaceData = pSurfaceDatabase->GetSurfaceIndex( solid.surfaceprop ); if ( surfaceData < 0 ) surfaceData = pSurfaceDatabase->GetSurfaceIndex( "default" ); solid.params.pName = params.pStudioHdr->name; ragdoll.list[ragdoll.listCount].pObject = pPhysEnv->CreatePolyObject( params.pCollide->solids[solid.index], surfaceData, vec3_origin, vec3_angle, &solid.params ); ragdoll.list[ragdoll.listCount].pObject->SetPositionMatrix( params.pCurrentBones[boneIndex], true ); ragdoll.list[ragdoll.listCount].parentIndex = -1; ragdoll.listCount++; } else { Msg( "CRagdollProp::CreateObjects: Couldn't Lookup Bone %s\n", solid.name ); } } } else if ( !strcmpi( pBlock, "ragdollconstraint" ) ) { constraint_ragdollparams_t constraint; pParse->ParseRagdollConstraint( &constraint, NULL ); if ( constraint.childIndex >= 0 && constraint.parentIndex >= 0 ) { Assert(constraint.childIndex<ragdoll.listCount); ragdollelement_t &childElement = ragdoll.list[constraint.childIndex]; // save parent index childElement.parentIndex = constraint.parentIndex; if ( params.jointFrictionScale > 0 ) { for ( int k = 0; k < 3; k++ ) { constraint.axes[k].torque *= params.jointFrictionScale; } } // this parent/child pair is not usually a parent/child pair in the skeleton. There // are often bones in between that are collapsed for simulation. So we need to compute // the transform. Studio_CalcBoneToBoneTransform( params.pStudioHdr, ragdoll.boneIndex[constraint.childIndex], ragdoll.boneIndex[constraint.parentIndex], constraint.constraintToAttached ); MatrixGetColumn( constraint.constraintToAttached, 3, childElement.originParentSpace ); // UNDONE: We could transform the constraint limit axes relative to the bone space // using this data. Do we need that feature? SetIdentityMatrix( constraint.constraintToReference ); pPhysEnv->DisableCollisions( ragdoll.list[constraint.parentIndex].pObject, childElement.pObject ); childElement.pConstraint = pPhysEnv->CreateRagdollConstraint( childElement.pObject, ragdoll.list[constraint.parentIndex].pObject, ragdoll.pGroup, constraint ); } } else { pParse->SkipBlock(); } } pPhysCollision->VPhysicsKeyParserDestroy( pParse ); }
void Initialize(void) { /* Set background (clear) color to dark blue */ glClearColor(0.0, 0.0, 0.4, 0.0); /* Enable depth testing */ glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LESS); /* Transform matrix for the bar in the middle*/ /*copy objects into buffer*/ memcpy(vertex_buffer_data1, vertex_octagon, sizeof(vertex_octagon)); memcpy(vertex_buffer_data2, vertex_pyramid, sizeof(vertex_pyramid)); memcpy(vertex_buffer_data3, vertex_mainBar, sizeof(vertex_mainBar)); memcpy(vertex_buffer_data4, vertex_cube, sizeof(vertex_cube)); memcpy(vertex_buffer_data5, vertex_cube, sizeof(vertex_cube)); memcpy(vertex_buffer_data6, vertex_cube, sizeof(vertex_cube)); memcpy(vertex_buffer_data7, vertex_cube, sizeof(vertex_cube)); /* memcpy(color_buffer_data1, color_octagon, sizeof(color_octagon)); memcpy(color_buffer_data2, color_pyramid, sizeof(color_pyramid)); memcpy(color_buffer_data3, color_mainBar, sizeof(color_mainBar)); memcpy(color_buffer_data4, color_cube, sizeof(color_cube)); memcpy(color_buffer_data5, color_cube, sizeof(color_cube)); memcpy(color_buffer_data6, color_cube, sizeof(color_cube)); memcpy(color_buffer_data7, color_cube, sizeof(color_cube)); */ memcpy(index_buffer_data1, index_octagon, sizeof(index_octagon)); memcpy(index_buffer_data2, index_pyramid, sizeof(index_pyramid)); memcpy(index_buffer_data3, index_mainBar, sizeof(index_mainBar)); memcpy(index_buffer_data4, index_cube, sizeof(index_cube)); memcpy(index_buffer_data5, index_cube, sizeof(index_cube)); memcpy(index_buffer_data6, index_cube, sizeof(index_cube)); memcpy(index_buffer_data7, index_cube, sizeof(index_cube)); /* Setup vertex, color, and index buffer objects */ SetupDataBuffers(); /* Setup shaders and shader program */ CreateShaderProgram(); /* Initialize matrices */ SetIdentityMatrix(ProjectionMatrix); SetIdentityMatrix(ViewMatrix); int i = 0; for(;i<OBJECTS;i++) { SetIdentityMatrix(ModelMatrix[i]); } printf("DEBUG: adding light-source ...\n"); /* Add lighting source to the code */ glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, GL_TRUE); glEnable(GL_LIGHTING); glEnable(GL_COLOR_MATERIAL); glEnable(GL_LIGHT0); // adds first light-source to the scene GLfloat ambientLight[] = {0.2, 0.2, 0.2, 1.0}; GLfloat diffuseLight[] = {0.8, 0.8, 0.8, 1.0}; GLfloat specularLight[]= {1.0, 1.0, 1.0, 1.0}; GLfloat positionLight[]= {5.0, 0.0, 0.0, 0.0}; //glShadeModel(GL_SMOOTH); glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight); glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight); glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight); glLightfv(GL_LIGHT0, GL_POSITION, positionLight); GLfloat black[] = {0.0, 0.0, 0.0, 1.0}; GLfloat green[] = {0.0, 1.0, 0.0, 1.0}; GLfloat white[] = {1.0, 1.0, 1.0, 1.0}; glMaterialfv(GL_FRONT, GL_AMBIENT, green); glMaterialfv(GL_FRONT, GL_DIFFUSE, green); glMaterialfv(GL_FRONT, GL_SPECULAR, white); glMaterialf(GL_FRONT, GL_SHININESS, 60.0); printf("DEBUG: finished adding light-source\n"); /* Set projection transform */ float fovy = 45.0; float aspect = 1.0; float nearPlane = 1.0; float farPlane = 50.0; SetPerspectiveMatrix(fovy, aspect, nearPlane, farPlane, ProjectionMatrix); /* Set viewing transform */ float camera_disp = -10.0; SetTranslation(0.0, 0.0, camera_disp, ViewMatrix); /* Translate and rotate cube */ SetTranslation(0, 0, 0, TranslateOrigin); SetRotationX(0.0, RotateX); SetRotationZ(0.0, RotateZ); /* Translate down */ SetTranslation(0, -sqrtf(sqrtf(2.0) * 1.0), 0, TranslateDown); /* Initial transformation matrix */ MultiplyMatrix(RotateX, TranslateOrigin, InitialTransform); MultiplyMatrix(RotateZ, InitialTransform, InitialTransform); }
OSErr SpriteUtils_SetSpriteData (QTAtomContainer theSprite, Point *theLocation, short *theVisible, short *theLayer, short *theImageIndex, ModifierTrackGraphicsModeRecord *theGraphicsMode, StringPtr theSpriteName, QTAtomContainer theActionAtoms) { QTAtom myPropertyAtom; OSErr myErr = noErr; if (theSprite == NULL) return(paramErr); // set the sprite location data if (theLocation != NULL) { MatrixRecord myMatrix; SetIdentityMatrix(&myMatrix); myMatrix.matrix[2][0] = ((long)theLocation->h << 16); myMatrix.matrix[2][1] = ((long)theLocation->v << 16); EndianUtils_MatrixRecord_NtoB(&myMatrix); myPropertyAtom = QTFindChildByIndex(theSprite, kParentAtomIsContainer, kSpritePropertyMatrix, 1, NULL); if (myPropertyAtom == 0) myErr = QTInsertChild(theSprite, kParentAtomIsContainer, kSpritePropertyMatrix, 1, 0, sizeof(MatrixRecord), &myMatrix, NULL); else myErr = QTSetAtomData(theSprite, myPropertyAtom, sizeof(MatrixRecord), &myMatrix); if (myErr != noErr) goto bail; } // set the sprite visibility state if (theVisible != NULL) { short myVisible = *theVisible; myVisible = EndianS16_NtoB(myVisible); myPropertyAtom = QTFindChildByIndex(theSprite, kParentAtomIsContainer, kSpritePropertyVisible, 1, NULL); if (myPropertyAtom == 0) myErr = QTInsertChild(theSprite, kParentAtomIsContainer, kSpritePropertyVisible, 1, 0, sizeof(short), &myVisible, NULL); else myErr = QTSetAtomData(theSprite, myPropertyAtom, sizeof(short), &myVisible); if (myErr != noErr) goto bail; } // set the sprite layer if (theLayer != NULL) { short myLayer = *theLayer; myLayer = EndianS16_NtoB(myLayer); myPropertyAtom = QTFindChildByIndex(theSprite, 0, kSpritePropertyLayer, 1, NULL); if (myPropertyAtom == 0) myErr = QTInsertChild(theSprite, 0, kSpritePropertyLayer, 1, 0, sizeof(short), &myLayer, NULL); else myErr = QTSetAtomData(theSprite, myPropertyAtom, sizeof(short), &myLayer); if (myErr != noErr) goto bail; } // set the sprite image index if (theImageIndex != NULL) { short myImageIndex = *theImageIndex; myImageIndex = EndianS16_NtoB(myImageIndex); myPropertyAtom = QTFindChildByIndex(theSprite, kParentAtomIsContainer, kSpritePropertyImageIndex, 1, NULL); if (myPropertyAtom == 0) myErr = QTInsertChild(theSprite, kParentAtomIsContainer, kSpritePropertyImageIndex, 1, 0, sizeof(short), &myImageIndex, NULL); else myErr = QTSetAtomData(theSprite, myPropertyAtom, sizeof(short), &myImageIndex); if (myErr != noErr) goto bail; } // set the sprite graphics mode if (theGraphicsMode != NULL) { ModifierTrackGraphicsModeRecord myGraphicsMode; myGraphicsMode.graphicsMode = EndianU32_NtoB(theGraphicsMode->graphicsMode); myGraphicsMode.opColor.red = EndianU16_NtoB(theGraphicsMode->opColor.red); myGraphicsMode.opColor.green = EndianU16_NtoB(theGraphicsMode->opColor.green); myGraphicsMode.opColor.blue = EndianU16_NtoB(theGraphicsMode->opColor.blue); myPropertyAtom = QTFindChildByIndex(theSprite, kParentAtomIsContainer, kSpritePropertyGraphicsMode, 1, NULL); if (myPropertyAtom == 0) myErr = QTInsertChild(theSprite, kParentAtomIsContainer, kSpritePropertyGraphicsMode, 1, 0, sizeof(myGraphicsMode), &myGraphicsMode, NULL); else myErr = QTSetAtomData(theSprite, myPropertyAtom, sizeof(myGraphicsMode), &myGraphicsMode); if (myErr != noErr) goto bail; } // set the sprite name if (theSpriteName != NULL) { QTAtom mySpriteNameAtom; mySpriteNameAtom = QTFindChildByIndex(theSprite, kParentAtomIsContainer, kSpriteNameAtomType, 1, NULL); if (mySpriteNameAtom == 0) myErr = QTInsertChild(theSprite, kParentAtomIsContainer, kSpriteNameAtomType, 1, 0, theSpriteName[0] + 1, theSpriteName, NULL); else myErr = QTSetAtomData(theSprite, mySpriteNameAtom, theSpriteName[0] + 1, theSpriteName); if (myErr != noErr) goto bail; } // set the action atoms if (theActionAtoms != NULL) myErr = QTInsertChildren(theSprite, kParentAtomIsContainer, theActionAtoms); bail: if ((myErr != noErr) && (theSprite != NULL)) QTRemoveChildren(theSprite, 0); return(myErr); }
void ResetReconstruction( INuiFusionReconstruction* pReconstruction, Matrix4* pMat ) { std::cout << "Reset Reconstruction" << std::endl; SetIdentityMatrix( *pMat ); pReconstruction->ResetReconstruction( pMat, nullptr ); }
int _tmain( int argc, _TCHAR* argv[] ) { cv::setUseOptimized( true ); // Sensor IKinectSensor* pSensor; HRESULT hResult = S_OK; hResult = GetDefaultKinectSensor( &pSensor ); if( FAILED( hResult ) ){ std::cerr << "Error : GetDefaultKinectSensor" << std::endl; return -1; } hResult = pSensor->Open(); if( FAILED( hResult ) ){ std::cerr << "Error : IKinectSensor::Open()" << std::endl; return -1; } // Source IDepthFrameSource* pDepthSource; hResult = pSensor->get_DepthFrameSource( &pDepthSource ); if( FAILED( hResult ) ){ std::cerr << "Error : IKinectSensor::get_DepthFrameSource()" << std::endl; return -1; } // Reader IDepthFrameReader* pDepthReader; hResult = pDepthSource->OpenReader( &pDepthReader ); if( FAILED( hResult ) ){ std::cerr << "Error : IDepthFrameSource::OpenReader()" << std::endl; return -1; } // Description IFrameDescription* pDescription; hResult = pDepthSource->get_FrameDescription( &pDescription ); if( FAILED( hResult ) ){ std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl; return -1; } int width = 0; int height = 0; pDescription->get_Width( &width ); // 512 pDescription->get_Height( &height ); // 424 unsigned int bufferSize = width * height * sizeof( unsigned short ); cv::Mat bufferMat( height, width, CV_16UC1 ); cv::Mat depthMat( height, width, CV_8UC1 ); cv::namedWindow( "Depth" ); // Coordinate Mapper ICoordinateMapper* pCoordinateMapper; hResult = pSensor->get_CoordinateMapper( &pCoordinateMapper ); if( FAILED( hResult ) ){ std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl; return -1; } // Create Reconstruction NUI_FUSION_RECONSTRUCTION_PARAMETERS reconstructionParameter; reconstructionParameter.voxelsPerMeter = 256; reconstructionParameter.voxelCountX = 512; reconstructionParameter.voxelCountY = 384; reconstructionParameter.voxelCountZ = 512; Matrix4 worldToCameraTransform; SetIdentityMatrix( worldToCameraTransform ); INuiFusionReconstruction* pReconstruction; hResult = NuiFusionCreateReconstruction( &reconstructionParameter, NUI_FUSION_RECONSTRUCTION_PROCESSOR_TYPE_AMP, -1, &worldToCameraTransform, &pReconstruction ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionCreateReconstruction()" << std::endl; return -1; } // Create Image Frame // Depth NUI_FUSION_IMAGE_FRAME* pDepthFloatImageFrame; hResult = NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE_FLOAT, width, height, nullptr, &pDepthFloatImageFrame ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionCreateImageFrame( FLOAT )" << std::endl; return -1; } // SmoothDepth NUI_FUSION_IMAGE_FRAME* pSmoothDepthFloatImageFrame; hResult = NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE_FLOAT, width, height, nullptr, &pSmoothDepthFloatImageFrame ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionCreateImageFrame( FLOAT )" << std::endl; return -1; } // Point Cloud NUI_FUSION_IMAGE_FRAME* pPointCloudImageFrame; hResult = NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE_POINT_CLOUD, width, height, nullptr, &pPointCloudImageFrame ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionCreateImageFrame( POINT_CLOUD )" << std::endl; return -1; } // Surface NUI_FUSION_IMAGE_FRAME* pSurfaceImageFrame; hResult = NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE_COLOR, width, height, nullptr, &pSurfaceImageFrame ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionCreateImageFrame( COLOR )" << std::endl; return -1; } // Normal NUI_FUSION_IMAGE_FRAME* pNormalImageFrame; hResult = NuiFusionCreateImageFrame( NUI_FUSION_IMAGE_TYPE_COLOR, width, height, nullptr, &pNormalImageFrame ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionCreateImageFrame( COLOR )" << std::endl; return -1; } cv::namedWindow( "Surface" ); cv::namedWindow( "Normal" ); while( 1 ){ // Frame IDepthFrame* pDepthFrame = nullptr; hResult = pDepthReader->AcquireLatestFrame( &pDepthFrame ); if( SUCCEEDED( hResult ) ){ hResult = pDepthFrame->AccessUnderlyingBuffer( &bufferSize, reinterpret_cast<UINT16**>( &bufferMat.data ) ); if( SUCCEEDED( hResult ) ){ bufferMat.convertTo( depthMat, CV_8U, -255.0f / 8000.0f, 255.0f ); hResult = pReconstruction->DepthToDepthFloatFrame( reinterpret_cast<UINT16*>( bufferMat.data ), width * height * sizeof( UINT16 ), pDepthFloatImageFrame, NUI_FUSION_DEFAULT_MINIMUM_DEPTH/* 0.5[m] */, NUI_FUSION_DEFAULT_MAXIMUM_DEPTH/* 8.0[m] */, true ); if( FAILED( hResult ) ){ std::cerr << "Error :INuiFusionReconstruction::DepthToDepthFloatFrame()" << std::endl; return -1; } } } SafeRelease( pDepthFrame ); // Smooting Depth Image Frame hResult = pReconstruction->SmoothDepthFloatFrame( pDepthFloatImageFrame, pSmoothDepthFloatImageFrame, 1, 0.04f ); if( FAILED( hResult ) ){ std::cerr << "Error :INuiFusionReconstruction::SmoothDepthFloatFrame" << std::endl; return -1; } // Reconstruction Process pReconstruction->GetCurrentWorldToCameraTransform( &worldToCameraTransform ); hResult = pReconstruction->ProcessFrame( pSmoothDepthFloatImageFrame, NUI_FUSION_DEFAULT_ALIGN_ITERATION_COUNT, NUI_FUSION_DEFAULT_INTEGRATION_WEIGHT, nullptr, &worldToCameraTransform ); if( FAILED( hResult ) ){ static int errorCount = 0; errorCount++; if( errorCount >= 100 ) { errorCount = 0; ResetReconstruction( pReconstruction, &worldToCameraTransform ); } } // Calculate Point Cloud hResult = pReconstruction->CalculatePointCloud( pPointCloudImageFrame, &worldToCameraTransform ); if( FAILED( hResult ) ){ std::cerr << "Error : CalculatePointCloud" << std::endl; return -1; } // Shading Point Clouid Matrix4 worldToBGRTransform = { 0.0f }; worldToBGRTransform.M11 = reconstructionParameter.voxelsPerMeter / reconstructionParameter.voxelCountX; worldToBGRTransform.M22 = reconstructionParameter.voxelsPerMeter / reconstructionParameter.voxelCountY; worldToBGRTransform.M33 = reconstructionParameter.voxelsPerMeter / reconstructionParameter.voxelCountZ; worldToBGRTransform.M41 = 0.5f; worldToBGRTransform.M42 = 0.5f; worldToBGRTransform.M43 = 0.0f; worldToBGRTransform.M44 = 1.0f; hResult = NuiFusionShadePointCloud( pPointCloudImageFrame, &worldToCameraTransform, &worldToBGRTransform, pSurfaceImageFrame, pNormalImageFrame ); if( FAILED( hResult ) ){ std::cerr << "Error : NuiFusionShadePointCloud" << std::endl; return -1; } cv::Mat surfaceMat( height, width, CV_8UC4, pSurfaceImageFrame->pFrameBuffer->pBits ); cv::Mat normalMat( height, width, CV_8UC4, pNormalImageFrame->pFrameBuffer->pBits ); cv::imshow( "Depth", depthMat ); cv::imshow( "Surface", surfaceMat ); cv::imshow( "Normal", normalMat ); int key = cv::waitKey( 30 ); if( key == VK_ESCAPE ){ break; } else if( key == 'r' ){ ResetReconstruction( pReconstruction, &worldToCameraTransform ); } } SafeRelease( pDepthSource ); SafeRelease( pDepthReader ); SafeRelease( pDescription ); SafeRelease( pCoordinateMapper ); SafeRelease( pReconstruction ); NuiFusionReleaseImageFrame( pDepthFloatImageFrame ); NuiFusionReleaseImageFrame( pSmoothDepthFloatImageFrame ); NuiFusionReleaseImageFrame( pPointCloudImageFrame ); NuiFusionReleaseImageFrame( pSurfaceImageFrame ); NuiFusionReleaseImageFrame( pNormalImageFrame ); if( pSensor ){ pSensor->Close(); } SafeRelease( pSensor ); cv::destroyAllWindows(); return 0; }