template <class T> void GaussNewton<T>::ComputeDeltas(const DMatrix &jac, const DArray &res, DMatrix &dc) const { SVD svd; double eig; DMatrix resm(1, res), resmt, jact, jacsq, u, e, vt, inv; TransposeMatrix(jac, jact); MultiplyMatrices(jact, jac, jacsq); //cerr << "SVD Input Matrix:" << endl; //PrintMatrix(jacsq); svd(jacsq, u, e, vt); svd.ComputeInverse(u, e , vt, inv); TransposeMatrix(resm, resmt); svd.ComputeTriProduct(inv, jact, resmt, dc); }
void VR_SetMatrices() { vec3_t temp, orientation, position; ovrMatrix4f projection; // Calculat HMD projection matrix and view offset position projection = TransposeMatrix(ovrMatrix4f_Projection(hmd->DefaultEyeFov[current_eye->index], 4, gl_farclip.value, ovrProjection_RightHanded)); // We need to scale the view offset position to quake units and rotate it by the current input angles (viewangle - eye orientation) QuatToYawPitchRoll(current_eye->pose.Orientation, orientation); temp[0] = -current_eye->pose.Position.z * meters_to_units; temp[1] = -current_eye->pose.Position.x * meters_to_units; temp[2] = current_eye->pose.Position.y * meters_to_units; Vec3RotateZ(temp, (r_refdef.viewangles[YAW]-orientation[YAW])*M_PI_DIV_180, position); // Set OpenGL projection and view matrices glMatrixMode(GL_PROJECTION); glLoadMatrixf((GLfloat*)projection.M); glMatrixMode(GL_MODELVIEW); glLoadIdentity (); glRotatef (-90, 1, 0, 0); // put Z going up glRotatef (90, 0, 0, 1); // put Z going up glRotatef (-r_refdef.viewangles[PITCH], 0, 1, 0); glRotatef (-r_refdef.viewangles[ROLL], 1, 0, 0); glRotatef (-r_refdef.viewangles[YAW], 0, 0, 1); glTranslatef (-r_refdef.vieworg[0] -position[0], -r_refdef.vieworg[1]-position[1], -r_refdef.vieworg[2]-position[2]); }
void CCharShape::AdjustOrientation (CControl *ctrl, double dtime, double dist_from_surface, TVector3 surf_nml) { TVector3 new_x, new_y, new_z; TMatrix cob_mat, inv_cob_mat; TMatrix rot_mat; TQuaternion new_orient; double time_constant; static TVector3 minus_z_vec = { 0, 0, -1}; static TVector3 y_vec = { 0, 1, 0 }; if (dist_from_surface > 0) { new_y = ScaleVector (1, ctrl->cvel); NormVector (&new_y); new_z = ProjectToPlane (new_y, MakeVector(0, -1, 0)); NormVector (&new_z); new_z = AdjustRollvector (ctrl, ctrl->cvel, new_z); } else { new_z = ScaleVector (-1, surf_nml); new_z = AdjustRollvector (ctrl, ctrl->cvel, new_z); new_y = ProjectToPlane (surf_nml, ScaleVector (1, ctrl->cvel)); NormVector(&new_y); } new_x = CrossProduct (new_y, new_z); MakeBasismatrix_Inv (cob_mat, inv_cob_mat, new_x, new_y, new_z); new_orient = MakeQuaternionFromMatrix (cob_mat); if (!ctrl->orientation_initialized) { ctrl->orientation_initialized = true; ctrl->corientation = new_orient; } time_constant = dist_from_surface > 0 ? TO_AIR_TIME : TO_TIME; ctrl->corientation = InterpolateQuaternions ( ctrl->corientation, new_orient, min (dtime / time_constant, 1.0)); ctrl->plane_nml = RotateVector (ctrl->corientation, minus_z_vec); ctrl->cdirection = RotateVector (ctrl->corientation, y_vec); MakeMatrixFromQuaternion (cob_mat, ctrl->corientation); // Trick rotations new_y = MakeVector (cob_mat[1][0], cob_mat[1][1], cob_mat[1][2]); RotateAboutVectorMatrix (rot_mat, new_y, (ctrl->roll_factor * 360)); MultiplyMatrices (cob_mat, rot_mat, cob_mat); new_x = MakeVector (cob_mat[0][0], cob_mat[0][1], cob_mat[0][2]); RotateAboutVectorMatrix (rot_mat, new_x, ctrl->flip_factor * 360); MultiplyMatrices (cob_mat, rot_mat, cob_mat); TransposeMatrix (cob_mat, inv_cob_mat); TransformNode (0, cob_mat, inv_cob_mat); }
int main() { clock_t time = clock(); IFileReader* reader = new PokFileReader(); IFileWriter* writer = new FileWriter(); Helper* helper = new Helper(reader, writer); std::vector<Patient> group1 = helper->ReadGroup("ALLD2", "D2", ".POK", 60); std::vector<Patient> group2 = helper->ReadGroup("ALLD3", "D3", ".POK", 42); IMeasureCalculator* measureCalculator = new PetuninMeasureCalculator(); Calculator* calculator = new Calculator(group1, group2, measureCalculator); std::vector<std::vector<double>> averageMatrix = calculator->AverageMatrix(15); averageMatrix = TransposeMatrix(averageMatrix); writer->WriteMatrix("result.txt", averageMatrix); delete calculator; delete measureCalculator; delete helper; delete reader; delete writer; //Do stuff time = clock() - time; double ms = double(time) / CLOCKS_PER_SEC * 1000; std::cout << "Finished!\nTime elapsed: " << ms << " ms" << std::endl; std::cin.get(); return 0; }
/* ================== CM_TransformedBoxTrace Handles offseting and rotation of the end points for moving and rotating entities ================== */ void CM_TransformedBoxTrace(trace_t *results, const vec3_t start, const vec3_t end, const vec3_t mins, const vec3_t maxs, clipHandle_t model, int brushmask, const vec3_t origin, const vec3_t angles, int capsule) { trace_t trace; vec3_t start_l, end_l; qboolean rotated; vec3_t offset; vec3_t symetricSize[2]; vec3_t matrix[3], transpose[3]; int i; float halfwidth; float halfheight; float t; sphere_t sphere; if (!mins) { mins = vec3_origin; } if (!maxs) { maxs = vec3_origin; } // adjust so that mins and maxs are always symetric, which // avoids some complications with plane expanding of rotated bmodels for (i = 0 ; i < 3 ; i++) { offset[i] = (mins[i] + maxs[i]) * 0.5; symetricSize[0][i] = mins[i] - offset[i]; symetricSize[1][i] = maxs[i] - offset[i]; start_l[i] = start[i] + offset[i]; end_l[i] = end[i] + offset[i]; } // subtract origin offset VectorSubtract(start_l, origin, start_l); VectorSubtract(end_l, origin, end_l); // rotate start and end into the models frame of reference if (model != BOX_MODEL_HANDLE && (angles[0] || angles[1] || angles[2])) { rotated = qtrue; } else { rotated = qfalse; } halfwidth = symetricSize[1][0]; halfheight = symetricSize[1][2]; sphere.use = capsule; sphere.radius = (halfwidth > halfheight) ? halfheight : halfwidth; sphere.halfheight = halfheight; t = halfheight - sphere.radius; if (rotated) { // rotation on trace line (start-end) instead of rotating the bmodel // NOTE: This is still incorrect for bounding boxes because the actual bounding // box that is swept through the model is not rotated. We cannot rotate // the bounding box or the bmodel because that would make all the brush // bevels invalid. // However this is correct for capsules since a capsule itself is rotated too. CreateRotationMatrix(angles, matrix); RotatePoint(start_l, matrix); RotatePoint(end_l, matrix); // rotated sphere offset for capsule sphere.offset[0] = matrix[0][2] * t; sphere.offset[1] = -matrix[1][2] * t; sphere.offset[2] = matrix[2][2] * t; } else { VectorSet(sphere.offset, 0, 0, t); } // sweep the box through the model CM_Trace(&trace, start_l, end_l, symetricSize[0], symetricSize[1], model, origin, brushmask, capsule, &sphere); // if the bmodel was rotated and there was a collision if (rotated && trace.fraction != 1.0) { // rotation of bmodel collision plane TransposeMatrix(matrix, transpose); RotatePoint(trace.plane.normal, transpose); } // re-calculate the end position of the trace because the trace.endpos // calculated by CM_Trace could be rotated and have an offset trace.endpos[0] = start[0] + trace.fraction * (end[0] - start[0]); trace.endpos[1] = start[1] + trace.fraction * (end[1] - start[1]); trace.endpos[2] = start[2] + trace.fraction * (end[2] - start[2]); *results = trace; }
void LoadAnimationDataFromCollada( const char* fileName, ArmatureKeyFrame* keyframe, Armature* armature ) { tinyxml2::XMLDocument colladaDoc; colladaDoc.LoadFile( fileName ); tinyxml2::XMLNode* animationNode = colladaDoc.FirstChildElement( "COLLADA" )->FirstChildElement( "library_animations" )->FirstChild(); while( animationNode != NULL ) { //Desired data: what bone, and what local transform to it occurs Mat4 boneLocalTransform; Bone* targetBone = NULL; //Parse the target attribute from the XMLElement for channel, and get the bone it corresponds to const char* transformName = animationNode->FirstChildElement( "channel" )->Attribute( "target" ); size_t nameLen = strlen( transformName ); char* transformNameCopy = (char*)alloca( nameLen ); strcpy( transformNameCopy, transformName ); char* nameEnd = transformNameCopy; while( *nameEnd != '/' && *nameEnd != 0 ) { nameEnd++; } memset( transformNameCopy, 0, nameLen ); nameLen = nameEnd - transformNameCopy; memcpy( transformNameCopy, transformName, nameLen ); for( uint8 boneIndex = 0; boneIndex < armature->boneCount; boneIndex++ ) { if( strcmp( armature->bones[ boneIndex ].name, transformNameCopy ) == 0 ) { targetBone = &armature->bones[ boneIndex ]; break; } } //Parse matrix data, and extract first keyframe data tinyxml2::XMLNode* transformMatrixElement = animationNode->FirstChild()->NextSibling(); const char* matrixTransformData = transformMatrixElement->FirstChild()->FirstChild()->Value(); size_t transformDataLen = strlen( matrixTransformData ) + 1; char* transformDataCopy = (char*)alloca( transformDataLen * sizeof( char ) ); memset( transformDataCopy, 0, transformDataLen ); memcpy( transformDataCopy, matrixTransformData, transformDataLen ); int count = 0; transformMatrixElement->FirstChildElement()->QueryAttribute( "count", &count ); float* rawTransformData = (float*)alloca( count * sizeof(float) ); memset( rawTransformData, 0, count * sizeof(float) ); TextToNumberConversion( transformDataCopy, rawTransformData ); memcpy( &boneLocalTransform.m[0][0], &rawTransformData[0], 16 * sizeof(float) ); //Save data in BoneKeyFrame struct boneLocalTransform = TransposeMatrix( boneLocalTransform ); if( targetBone == armature->rootBone ) { Mat4 correction; correction.m[0][0] = 1.0f; correction.m[0][1] = 0.0f; correction.m[0][2] = 0.0f; correction.m[0][3] = 0.0f; correction.m[1][0] = 0.0f; correction.m[1][1] = 0.0f; correction.m[1][2] = -1.0f; correction.m[1][3] = 0.0f; correction.m[2][0] = 0.0f; correction.m[2][1] = 1.0f; correction.m[2][2] = 0.0f; correction.m[2][3] = 0.0f; correction.m[3][0] = 0.0f; correction.m[3][1] = 0.0f; correction.m[3][2] = 0.0f; correction.m[3][3] = 1.0f; boneLocalTransform = MultMatrix( boneLocalTransform, correction ); } BoneKeyFrame* key = &keyframe->targetBoneTransforms[ targetBone->boneIndex ]; key->combinedMatrix = boneLocalTransform; DecomposeMat4( boneLocalTransform, &key->scale, &key->rotation, &key->translation ); Mat4 m = Mat4FromComponents( key->scale, key->rotation, key->translation ); animationNode = animationNode->NextSibling(); } //Pre multiply bones with parents to save doing it during runtime struct { ArmatureKeyFrame* keyframe; void PremultiplyKeyFrame( Bone* target, Mat4 parentTransform ) { BoneKeyFrame* boneKey = &keyframe->targetBoneTransforms[ target->boneIndex ]; Mat4 netMatrix = MultMatrix( boneKey->combinedMatrix, parentTransform ); for( uint8 boneIndex = 0; boneIndex < target->childCount; boneIndex++ ) { PremultiplyKeyFrame( target->children[ boneIndex ], netMatrix ); } boneKey->combinedMatrix = netMatrix; DecomposeMat4( boneKey->combinedMatrix, &boneKey->scale, &boneKey->rotation, &boneKey->translation ); } }LocalRecursiveScope; LocalRecursiveScope.keyframe = keyframe; Mat4 i; SetToIdentity( &i ); LocalRecursiveScope.PremultiplyKeyFrame( armature->rootBone, i ); }
void LoadMeshDataFromDisk( const char* fileName, SlabSubsection_Stack* allocater, MeshGeometryData* storage, Armature* armature ) { tinyxml2::XMLDocument colladaDoc; colladaDoc.LoadFile( fileName ); //if( colladaDoc == NULL ) { //printf( "Could not load mesh: %s\n", fileName ); //return; //} tinyxml2::XMLElement* meshNode = colladaDoc.FirstChildElement( "COLLADA" )->FirstChildElement( "library_geometries" ) ->FirstChildElement( "geometry" )->FirstChildElement( "mesh" ); char* colladaTextBuffer = NULL; size_t textBufferLen = 0; uint16 vCount = 0; uint16 nCount = 0; uint16 uvCount = 0; uint16 indexCount = 0; float* rawColladaVertexData; float* rawColladaNormalData; float* rawColladaUVData; float* rawIndexData; ///Basic Mesh Geometry Data { tinyxml2::XMLNode* colladaVertexArray = meshNode->FirstChildElement( "source" ); tinyxml2::XMLElement* vertexFloatArray = colladaVertexArray->FirstChildElement( "float_array" ); tinyxml2::XMLNode* colladaNormalArray = colladaVertexArray->NextSibling(); tinyxml2::XMLElement* normalFloatArray = colladaNormalArray->FirstChildElement( "float_array" ); tinyxml2::XMLNode* colladaUVMapArray = colladaNormalArray->NextSibling(); tinyxml2::XMLElement* uvMapFloatArray = colladaUVMapArray->FirstChildElement( "float_array" ); tinyxml2::XMLElement* meshSrc = meshNode->FirstChildElement( "polylist" ); tinyxml2::XMLElement* colladaIndexArray = meshSrc->FirstChildElement( "p" ); int count; const char* colladaVertArrayVal = vertexFloatArray->FirstChild()->Value(); vertexFloatArray->QueryAttribute( "count", &count ); vCount = count; const char* colladaNormArrayVal = normalFloatArray->FirstChild()->Value(); normalFloatArray->QueryAttribute( "count", &count ); nCount = count; const char* colladaUVMapArrayVal = uvMapFloatArray->FirstChild()->Value(); uvMapFloatArray->QueryAttribute( "count", &count ); uvCount = count; const char* colladaIndexArrayVal = colladaIndexArray->FirstChild()->Value(); meshSrc->QueryAttribute( "count", &count ); //Assume this is already triangulated indexCount = count * 3 * 3; ///TODO: replace this with fmaxf? std::function< size_t (size_t, size_t) > sizeComparison = []( size_t size1, size_t size2 ) -> size_t { if( size1 >= size2 ) return size1; return size2; }; textBufferLen = strlen( colladaVertArrayVal ); textBufferLen = sizeComparison( strlen( colladaNormArrayVal ), textBufferLen ); textBufferLen = sizeComparison( strlen( colladaUVMapArrayVal ), textBufferLen ); textBufferLen = sizeComparison( strlen( colladaIndexArrayVal ), textBufferLen ); colladaTextBuffer = (char*)alloca( textBufferLen ); memset( colladaTextBuffer, 0, textBufferLen ); rawColladaVertexData = (float*)alloca( sizeof(float) * vCount ); rawColladaNormalData = (float*)alloca( sizeof(float) * nCount ); rawColladaUVData = (float*)alloca( sizeof(float) * uvCount ); rawIndexData = (float*)alloca( sizeof(float) * indexCount ); memset( rawColladaVertexData, 0, sizeof(float) * vCount ); memset( rawColladaNormalData, 0, sizeof(float) * nCount ); memset( rawColladaUVData, 0, sizeof(float) * uvCount ); memset( rawIndexData, 0, sizeof(float) * indexCount ); //Reading Vertex position data strcpy( colladaTextBuffer, colladaVertArrayVal ); TextToNumberConversion( colladaTextBuffer, rawColladaVertexData ); //Reading Normals data memset( colladaTextBuffer, 0, textBufferLen ); strcpy( colladaTextBuffer, colladaNormArrayVal ); TextToNumberConversion( colladaTextBuffer, rawColladaNormalData ); //Reading UV map data memset( colladaTextBuffer, 0, textBufferLen ); strcpy( colladaTextBuffer, colladaUVMapArrayVal ); TextToNumberConversion( colladaTextBuffer, rawColladaUVData ); //Reading index data memset( colladaTextBuffer, 0, textBufferLen ); strcpy( colladaTextBuffer, colladaIndexArrayVal ); TextToNumberConversion( colladaTextBuffer, rawIndexData ); } float* rawBoneWeightData = NULL; float* rawBoneIndexData = NULL; //Skinning Data { tinyxml2::XMLElement* libControllers = colladaDoc.FirstChildElement( "COLLADA" )->FirstChildElement( "library_controllers" ); if( libControllers == NULL ) goto skinningExit; tinyxml2::XMLElement* controllerElement = libControllers->FirstChildElement( "controller" ); if( controllerElement == NULL ) goto skinningExit; tinyxml2::XMLNode* vertexWeightDataArray = controllerElement->FirstChild()->FirstChild()->NextSibling()->NextSibling()->NextSibling(); tinyxml2::XMLNode* vertexBoneIndexDataArray = vertexWeightDataArray->NextSibling()->NextSibling(); tinyxml2::XMLNode* vCountArray = vertexBoneIndexDataArray->FirstChildElement( "vcount" ); tinyxml2::XMLNode* vArray = vertexBoneIndexDataArray->FirstChildElement( "v" ); const char* boneWeightsData = vertexWeightDataArray->FirstChild()->FirstChild()->Value(); const char* vCountArrayData = vCountArray->FirstChild()->Value(); const char* vArrayData = vArray->FirstChild()->Value(); float* colladaBoneWeightData = NULL; float* colladaBoneIndexData = NULL; float* colladaBoneInfluenceCounts = NULL; ///This is overkill, Collada stores ways less data usually, plus this still doesn't account for very complex models ///(e.g, lots of verts with more than MAXBONESPERVERT influencing position ) colladaBoneWeightData = (float*)alloca( sizeof(float) * MAXBONESPERVERT * vCount ); colladaBoneIndexData = (float*)alloca( sizeof(float) * MAXBONESPERVERT * vCount ); colladaBoneInfluenceCounts = (float*)alloca( sizeof(float) * MAXBONESPERVERT * vCount ); //Read bone weights data memset( colladaTextBuffer, 0, textBufferLen ); strcpy( colladaTextBuffer, boneWeightsData ); TextToNumberConversion( colladaTextBuffer, colladaBoneWeightData ); //Read bone index data memset( colladaTextBuffer, 0, textBufferLen ); strcpy( colladaTextBuffer, vArrayData ); TextToNumberConversion( colladaTextBuffer, colladaBoneIndexData ); //Read bone influence counts memset( colladaTextBuffer, 0, textBufferLen ); strcpy( colladaTextBuffer, vCountArrayData ); TextToNumberConversion( colladaTextBuffer, colladaBoneInfluenceCounts ); rawBoneWeightData = (float*)alloca( sizeof(float) * MAXBONESPERVERT * vCount ); rawBoneIndexData = (float*)alloca( sizeof(float) * MAXBONESPERVERT * vCount ); memset( rawBoneWeightData, 0, sizeof(float) * MAXBONESPERVERT * vCount ); memset( rawBoneIndexData, 0, sizeof(float) * MAXBONESPERVERT * vCount ); int colladaIndexIndirection = 0; int verticiesInfluenced = 0; vCountArray->Parent()->ToElement()->QueryAttribute( "count", &verticiesInfluenced ); for( uint16 i = 0; i < verticiesInfluenced; i++ ) { uint8 influenceCount = colladaBoneInfluenceCounts[i]; for( uint16 j = 0; j < influenceCount; j++ ) { uint16 boneIndex = colladaBoneIndexData[ colladaIndexIndirection++ ]; uint16 weightIndex = colladaBoneIndexData[ colladaIndexIndirection++ ]; rawBoneWeightData[ i * MAXBONESPERVERT + j ] = colladaBoneWeightData[ weightIndex ]; rawBoneIndexData[ i * MAXBONESPERVERT + j ] = boneIndex; } } } skinningExit: //Armature if( armature != NULL ) { tinyxml2::XMLElement* visualScenesNode = colladaDoc.FirstChildElement( "COLLADA" )->FirstChildElement( "library_visual_scenes" ) ->FirstChildElement( "visual_scene" )->FirstChildElement( "node" ); tinyxml2::XMLElement* armatureNode = NULL; //Step through scene heirarchy until start of armature is found while( visualScenesNode != NULL ) { if( visualScenesNode->FirstChildElement( "node" ) != NULL && visualScenesNode->FirstChildElement( "node" )->Attribute( "type", "JOINT" ) != NULL ) { armatureNode = visualScenesNode; break; } else { visualScenesNode = visualScenesNode->NextSibling()->ToElement(); } } if( armatureNode == NULL ) return; //Parsing basic bone data from XML std::function< Bone* ( tinyxml2::XMLElement*, Armature*, Bone* ) > ParseColladaBoneData = [&]( tinyxml2::XMLElement* boneElement, Armature* armature, Bone* parentBone ) -> Bone* { Bone* bone = &armature->bones[ armature->boneCount ]; bone->parent = parentBone; bone->currentTransform = &armature->boneTransforms[ armature->boneCount ]; SetToIdentity( bone->currentTransform ); bone->boneIndex = armature->boneCount; armature->boneCount++; strcpy( &bone->name[0], boneElement->Attribute( "sid" ) ); float matrixData[16]; char matrixTextData [512]; tinyxml2::XMLNode* matrixElement = boneElement->FirstChildElement("matrix"); strcpy( &matrixTextData[0], matrixElement->FirstChild()->ToText()->Value() ); TextToNumberConversion( matrixTextData, matrixData ); //Note: this is only local transform data, but its being saved in bind matrix for now Mat4 m; memcpy( &m.m[0][0], &matrixData[0], sizeof(float) * 16 ); bone->bindPose = TransposeMatrix( m ); if( parentBone == NULL ) { armature->rootBone = bone; } else { bone->bindPose = MultMatrix( parentBone->bindPose, bone->bindPose ); } bone->childCount = 0; tinyxml2::XMLElement* childBoneElement = boneElement->FirstChildElement( "node" ); while( childBoneElement != NULL ) { Bone* childBone = ParseColladaBoneData( childBoneElement, armature, bone ); bone->children[ bone->childCount++ ] = childBone; tinyxml2::XMLNode* siblingNode = childBoneElement->NextSibling(); if( siblingNode != NULL ) { childBoneElement = siblingNode->ToElement(); } else { childBoneElement = NULL; } }; return bone; }; armature->boneCount = 0; tinyxml2::XMLElement* boneElement = armatureNode->FirstChildElement( "node" ); ParseColladaBoneData( boneElement, armature, NULL ); //Parse inverse bind pose data from skinning section of XML { tinyxml2::XMLElement* boneNamesSource = colladaDoc.FirstChildElement( "COLLADA" )->FirstChildElement( "library_controllers" ) ->FirstChildElement( "controller" )->FirstChildElement( "skin" )->FirstChildElement( "source" ); tinyxml2::XMLElement* boneBindPoseSource = boneNamesSource->NextSibling()->ToElement(); char* boneNamesLocalCopy = NULL; float* boneMatriciesData = (float*)alloca( sizeof(float) * 16 * armature->boneCount ); const char* boneNameArrayData = boneNamesSource->FirstChild()->FirstChild()->Value(); const char* boneMatrixTextData = boneBindPoseSource->FirstChild()->FirstChild()->Value(); size_t nameDataLen = strlen( boneNameArrayData ); size_t matrixDataLen = strlen( boneMatrixTextData ); boneNamesLocalCopy = (char*)alloca( nameDataLen + 1 ); memset( boneNamesLocalCopy, 0, nameDataLen + 1 ); assert( textBufferLen > matrixDataLen ); memcpy( boneNamesLocalCopy, boneNameArrayData, nameDataLen ); memcpy( colladaTextBuffer, boneMatrixTextData, matrixDataLen ); TextToNumberConversion( colladaTextBuffer, boneMatriciesData ); char* nextBoneName = &boneNamesLocalCopy[0]; for( uint8 matrixIndex = 0; matrixIndex < armature->boneCount; matrixIndex++ ) { Mat4 matrix; memcpy( &matrix.m[0], &boneMatriciesData[matrixIndex * 16], sizeof(float) * 16 ); char boneName [32]; char* boneNameEnd = nextBoneName; do { boneNameEnd++; } while( *boneNameEnd != ' ' && *boneNameEnd != 0 ); size_t charCount = boneNameEnd - nextBoneName; memset( boneName, 0, sizeof( char ) * 32 ); memcpy( boneName, nextBoneName, charCount ); nextBoneName = boneNameEnd + 1; Bone* targetBone = NULL; for( uint8 boneIndex = 0; boneIndex < armature->boneCount; boneIndex++ ) { Bone* bone = &armature->bones[ boneIndex ]; if( strcmp( bone->name, boneName ) == 0 ) { targetBone = bone; break; } } Mat4 correction; correction.m[0][0] = 1.0f; correction.m[0][1] = 0.0f; correction.m[0][2] = 0.0f; correction.m[0][3] = 0.0f; correction.m[1][0] = 0.0f; correction.m[1][1] = 0.0f; correction.m[1][2] = 1.0f; correction.m[1][3] = 0.0f; correction.m[2][0] = 0.0f; correction.m[2][1] = -1.0f; correction.m[2][2] = 0.0f; correction.m[2][3] = 0.0f; correction.m[3][0] = 0.0f; correction.m[3][1] = 0.0f; correction.m[3][2] = 0.0f; correction.m[3][3] = 1.0f; targetBone->invBindPose = TransposeMatrix( matrix ); targetBone->invBindPose = MultMatrix( correction, targetBone->invBindPose ); } } } //output to my version of storage storage->dataCount = 0; uint16 counter = 0; const uint32 vertCount = indexCount / 3; storage->vData = (Vec3*)AllocOnSubStack_Aligned( allocater, vertCount * sizeof( Vec3 ), 16 ); storage->uvData = (float*)AllocOnSubStack_Aligned( allocater, vertCount * 2 * sizeof( float ), 4 ); storage->normalData = (Vec3*)AllocOnSubStack_Aligned( allocater, vertCount * sizeof( Vec3 ), 4 ); storage->iData = (uint32*)AllocOnSubStack_Aligned( allocater, vertCount * sizeof( uint32 ), 4 ); if( rawBoneWeightData != NULL ) { storage->boneWeightData = (float*)AllocOnSubStack_Aligned( allocater, sizeof(float) * vertCount * MAXBONESPERVERT, 4 ); storage->boneIndexData = (uint32*)AllocOnSubStack_Aligned( allocater, sizeof(uint32) * vertCount * MAXBONESPERVERT, 4 ); } else { storage->boneWeightData = NULL; storage->boneIndexData = NULL; } while( counter < indexCount ) { Vec3 v, n; float uv_x, uv_y; uint16 vertIndex = rawIndexData[ counter++ ]; uint16 normalIndex = rawIndexData[ counter++ ]; uint16 uvIndex = rawIndexData[ counter++ ]; v.x = rawColladaVertexData[ vertIndex * 3 + 0 ]; v.z = -rawColladaVertexData[ vertIndex * 3 + 1 ]; v.y = rawColladaVertexData[ vertIndex * 3 + 2 ]; n.x = rawColladaNormalData[ normalIndex * 3 + 0 ]; n.z = -rawColladaNormalData[ normalIndex * 3 + 1 ]; n.y = rawColladaNormalData[ normalIndex * 3 + 2 ]; uv_x = rawColladaUVData[ uvIndex * 2 ]; uv_y = rawColladaUVData[ uvIndex * 2 + 1 ]; ///TODO: check for exact copies of data, use to index to first instance instead uint32 storageIndex = storage->dataCount; storage->vData[ storageIndex ] = v; storage->normalData[ storageIndex ] = n; storage->uvData[ storageIndex * 2 ] = uv_x; storage->uvData[ storageIndex * 2 + 1 ] = uv_y; if( rawBoneWeightData != NULL ) { uint16 boneDataIndex = storage->dataCount * MAXBONESPERVERT; uint16 boneVertexIndex = vertIndex * MAXBONESPERVERT; for( uint8 i = 0; i < MAXBONESPERVERT; i++ ) { storage->boneWeightData[ boneDataIndex + i ] = rawBoneWeightData[ boneVertexIndex + i ]; storage->boneIndexData[ boneDataIndex + i ] = rawBoneIndexData[ boneVertexIndex + i ]; } } storage->iData[ storageIndex ] = storageIndex; storage->dataCount++; }; }
void CombatSubsystem::AimWeaponTag(const Vector& targetPos) { Vector aimAngles; Vector gunPos, gunForward, gunRight, gunUp; //currentEnemy = act->enemyManager->GetCurrentEnemy(); if (!_activeWeapon.weapon) return; _activeWeapon.weapon->setOrigin(); _activeWeapon.weapon->setAngles(); _activeWeapon.weapon->SetControllerAngles(WEAPONBONE_BARREL_TAG, vec_zero); GetGunPositionData(&gunPos, &gunForward, &gunRight, &gunUp); Vector mypos, myforward, myleft, myup; _activeWeapon.weapon->GetTag(WEAPONBONE_BARREL_TAG, &mypos, &myforward, &myleft, &myup); float gfToWorld[ 3 ][ 3 ]; float worldToGf[ 3 ][ 3 ]; gunForward.copyTo(gfToWorld[0]); gunRight.copyTo(gfToWorld[1]); gunUp.copyTo(gfToWorld[2]); TransposeMatrix(gfToWorld, worldToGf); auto barrelToEnemyVector = targetPos - gunPos; vec3_t barrelToEnemyVec3_t; barrelToEnemyVector.copyTo(barrelToEnemyVec3_t); vec3_t barrelToEnemyTransformedVec3_t; MatrixTransformVector(barrelToEnemyVec3_t, worldToGf, barrelToEnemyTransformedVec3_t); Vector barrelToEnemyTransformed(barrelToEnemyTransformedVec3_t); barrelToEnemyTransformed.normalize(); barrelToEnemyTransformed = barrelToEnemyTransformed.toAngles(); barrelToEnemyTransformed.EulerNormalize(); aimAngles = -barrelToEnemyTransformed; Vector aimUp, aimLeft, aimForward; float spreadX, spreadY; aimAngles.AngleVectors(&aimForward, &aimLeft, &aimUp); spreadX = GetDataForMyWeapon("spreadx"); spreadY = GetDataForMyWeapon("spready"); // figure the new projected impact point based upon computed spread if (_activeWeapon.weapon->GetFireType(FIRE_MODE1) == FT_PROJECTILE) { // Apply Spread aimForward = aimForward * _activeWeapon.weapon->GetRange(FIRE_MODE1) + aimLeft * G_CRandom(spreadX) + aimUp * G_CRandom(spreadY); } // after figuring spread location, re-normalize vectors aimForward.normalize(); aimAngles = aimForward.toAngles(); _activeWeapon.weapon->SetControllerTag(WEAPONBONE_BARREL_TAG, gi.Tag_NumForName(_activeWeapon.weapon->edict->s.modelindex, "tag_barrel")); _activeWeapon.weapon->SetControllerAngles(WEAPONBONE_BARREL_TAG, aimAngles); //Draw Trace if (g_showbullettrace->integer) { Vector test; GetGunPositionData(&gunPos, &gunForward); test = gunForward * 1000 + gunPos; G_DebugLine(gunPos, test, 1.0f, 0.0f, 0.0f, 1.0f); Vector barrelForward; aimAngles.AngleVectors(&barrelForward); } }
int sci_umfpack(char* fname, void* pvApiCtx) { SciErr sciErr; int mb = 0; int nb = 0; int i = 0; int num_A = 0; int num_b = 0; int mW = 0; int Case = 0; int stat = 0; SciSparse AA; CcsSparse A; int* piAddrA = NULL; int* piAddr2 = NULL; int* piAddrB = NULL; double* pdblBR = NULL; double* pdblBI = NULL; double* pdblXR = NULL; double* pdblXI = NULL; int iComplex = 0; int freepdblBI = 0; int mA = 0; // rows int nA = 0; // cols int iNbItem = 0; int* piNbItemRow = NULL; int* piColPos = NULL; double* pdblSpReal = NULL; double* pdblSpImg = NULL; /* umfpack stuff */ double Info[UMFPACK_INFO]; double* Control = NULL; void* Symbolic = NULL; void* Numeric = NULL; int* Wi = NULL; double* W = NULL; char* pStr = NULL; int iType2 = 0; int iTypeA = 0; int iTypeB = 0; /* Check numbers of input/output arguments */ CheckInputArgument(pvApiCtx, 3, 3); CheckOutputArgument(pvApiCtx, 1, 1); /* First get arg #2 : a string of length 1 */ sciErr = getVarAddressFromPosition(pvApiCtx, 2, &piAddr2); if (sciErr.iErr) { printError(&sciErr, 0); return 1; } sciErr = getVarType(pvApiCtx, piAddr2, &iType2); if (sciErr.iErr || iType2 != sci_strings) { printError(&sciErr, 0); Scierror(999, _("%s: Wrong type for input argument #%d: string expected.\n"), fname, 2); return 1; } if (getAllocatedSingleString(pvApiCtx, piAddr2, &pStr)) { return 1; } /* select Case 1 or 2 depending (of the first char of) the string ... */ if (pStr[0] == '\\') // compare pStr[0] with '\' { Case = 1; num_A = 1; num_b = 3; } else if (pStr[0] == '/') { Case = 2; num_A = 3; num_b = 1; } else { Scierror(999, _("%s: Wrong input argument #%d: '%s' or '%s' expected.\n"), fname, 2, "\\", "/"); FREE(pStr); return 1; } FREE(pStr); /* get A */ sciErr = getVarAddressFromPosition(pvApiCtx, num_A, &piAddrA); if (sciErr.iErr) { printError(&sciErr, 0); return 1; } sciErr = getVarType(pvApiCtx, piAddrA, &iTypeA); if (sciErr.iErr || iTypeA != sci_sparse) { printError(&sciErr, 0); Scierror(999, _("%s: Wrong type for input argument #%d: A sparse matrix expected.\n"), fname, 1); return 1; } if (isVarComplex(pvApiCtx, piAddrA)) { AA.it = 1; iComplex = 1; sciErr = getComplexSparseMatrix(pvApiCtx, piAddrA, &mA, &nA, &iNbItem, &piNbItemRow, &piColPos, &pdblSpReal, &pdblSpImg); } else { AA.it = 0; sciErr = getSparseMatrix(pvApiCtx, piAddrA, &mA, &nA, &iNbItem, &piNbItemRow, &piColPos, &pdblSpReal); } if (sciErr.iErr) { printError(&sciErr, 0); return 1; } // fill struct sparse AA.m = mA; AA.n = nA; AA.nel = iNbItem; AA.mnel = piNbItemRow; AA.icol = piColPos; AA.R = pdblSpReal; AA.I = pdblSpImg; if ( mA != nA || mA < 1 ) { Scierror(999, _("%s: Wrong size for input argument #%d.\n"), fname, num_A); return 1; } /* get B*/ sciErr = getVarAddressFromPosition(pvApiCtx, num_b, &piAddrB); if (sciErr.iErr) { printError(&sciErr, 0); return 1; } sciErr = getVarType(pvApiCtx, piAddrB, &iTypeB); if (sciErr.iErr || iTypeB != sci_matrix) { printError(&sciErr, 0); Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), fname, 3); return 1; } if (isVarComplex(pvApiCtx, piAddrB)) { iComplex = 1; sciErr = getComplexMatrixOfDouble(pvApiCtx, piAddrB, &mb, &nb, &pdblBR, &pdblBI); } else { sciErr = getMatrixOfDouble(pvApiCtx, piAddrB, &mb, &nb, &pdblBR); } if (sciErr.iErr) { printError(&sciErr, 0); return 1; } if ( (Case == 1 && ( mb != mA || nb < 1 )) || (Case == 2 && ( nb != mA || mb < 1 )) ) { Scierror(999, _("%s: Wrong size for input argument #%d.\n"), fname, num_b); return 1; } SciSparseToCcsSparse(&AA, &A); /* allocate memory for the solution x */ if (iComplex) { sciErr = allocComplexMatrixOfDouble(pvApiCtx, 4, mb, nb, &pdblXR, &pdblXI); } else { sciErr = allocMatrixOfDouble(pvApiCtx, 4, mb, nb, &pdblXR); } if (sciErr.iErr) { printError(&sciErr, 0); freeCcsSparse(A); return 1; } if (A.it == 1) { mW = 10 * mA; } else { mW = 5 * mA; } if (A.it == 1 && pdblBI == NULL) { int iSize = mb * nb * sizeof(double); pdblBI = (double*)MALLOC(iSize); memset(pdblBI, 0x00, iSize); freepdblBI = 1; } /* Now calling umfpack routines */ if (A.it == 1) { stat = umfpack_zi_symbolic(mA, nA, A.p, A.irow, A.R, A.I, &Symbolic, Control, Info); } else { stat = umfpack_di_symbolic(mA, nA, A.p, A.irow, A.R, &Symbolic, Control, Info); } if ( stat != UMFPACK_OK ) { Scierror(999, _("%s: An error occurred: %s: %s\n"), fname, _("symbolic factorization"), UmfErrorMes(stat)); freeCcsSparse(A); if (freepdblBI) { FREE(pdblBI); } return 1; } if (A.it == 1) { stat = umfpack_zi_numeric(A.p, A.irow, A.R, A.I, Symbolic, &Numeric, Control, Info); } else { stat = umfpack_di_numeric(A.p, A.irow, A.R, Symbolic, &Numeric, Control, Info); } if (A.it == 1) { umfpack_zi_free_symbolic(&Symbolic); } else { umfpack_di_free_symbolic(&Symbolic); } if ( stat != UMFPACK_OK ) { Scierror(999, _("%s: An error occurred: %s: %s\n"), fname, _("numeric factorization"), UmfErrorMes(stat)); if (A.it == 1) { umfpack_zi_free_numeric(&Numeric); } else { umfpack_di_free_numeric(&Numeric); } freeCcsSparse(A); if (freepdblBI) { FREE(pdblBI); } return 1; } /* allocate memory for umfpack_di_wsolve usage or umfpack_zi_wsolve usage*/ Wi = (int*)MALLOC(mA * sizeof(int)); W = (double*)MALLOC(mW * sizeof(double)); if ( Case == 1 ) /* x = A\b <=> Ax = b */ { if (A.it == 0) { for ( i = 0 ; i < nb ; i++ ) { umfpack_di_wsolve(UMFPACK_A, A.p, A.irow, A.R, &pdblXR[i * mb], &pdblBR[i * mb], Numeric, Control, Info, Wi, W); } if (isVarComplex(pvApiCtx, piAddrB)) { for ( i = 0 ; i < nb ; i++ ) { umfpack_di_wsolve(UMFPACK_A, A.p, A.irow, A.R, &pdblXI[i * mb], &pdblBI[i * mb], Numeric, Control, Info, Wi, W); } } } else /* A.it == 1 */ { for ( i = 0 ; i < nb ; i++ ) { umfpack_zi_wsolve(UMFPACK_A, A.p, A.irow, A.R, A.I, &pdblXR[i * mb], &pdblXI[i * mb], &pdblBR[i * mb], &pdblBI[i * mb], Numeric, Control, Info, Wi, W); } } } else /* Case == 2, x = b/A <=> x A = b <=> A.'x.' = b.' */ { if (A.it == 0) { TransposeMatrix(pdblBR, mb, nb, pdblXR); /* put b in x (with transposition) */ for ( i = 0 ; i < mb ; i++ ) { umfpack_di_wsolve(UMFPACK_At, A.p, A.irow, A.R, &pdblBR[i * nb], &pdblXR[i * nb], Numeric, Control, Info, Wi, W); /* the solutions are in br */ } TransposeMatrix(pdblBR, nb, mb, pdblXR); /* put now br in xr with transposition */ if (isVarComplex(pvApiCtx, piAddrB)) { TransposeMatrix(pdblBI, mb, nb, pdblXI); /* put b in x (with transposition) */ for ( i = 0 ; i < mb ; i++ ) { umfpack_di_wsolve(UMFPACK_At, A.p, A.irow, A.R, &pdblBI[i * nb], &pdblXI[i * nb], Numeric, Control, Info, Wi, W); /* the solutions are in bi */ } TransposeMatrix(pdblBI, nb, mb, pdblXI); /* put now bi in xi with transposition */ } } else /* A.it==1 */ { TransposeMatrix(pdblBR, mb, nb, pdblXR); TransposeMatrix(pdblBI, mb, nb, pdblXI); for ( i = 0 ; i < mb ; i++ ) { umfpack_zi_wsolve(UMFPACK_Aat, A.p, A.irow, A.R, A.I, &pdblBR[i * nb], &pdblBI[i * nb], &pdblXR[i * nb], &pdblXI[i * nb], Numeric, Control, Info, Wi, W); } TransposeMatrix(pdblBR, nb, mb, pdblXR); TransposeMatrix(pdblBI, nb, mb, pdblXI); } } if (A.it == 1) { umfpack_zi_free_numeric(&Numeric); } else { umfpack_di_free_numeric(&Numeric); } if (piNbItemRow != NULL) { FREE(piNbItemRow); } if (piColPos != NULL) { FREE(piColPos); } if (pdblSpReal != NULL) { FREE(pdblSpReal); } if (pdblSpImg != NULL) { FREE(pdblSpImg); } FREE(W); FREE(Wi); if (freepdblBI) { FREE(pdblBI); } freeCcsSparse(A); AssignOutputVariable(pvApiCtx, 1) = 4; ReturnArguments(pvApiCtx); return 0; }
// Here's the main interpolate function, using // Least Squares AutoRegression (LSAR): void InterpolateAudio(float *buffer, int len, int firstBad, int numBad) { int N = len; int i, row, col; wxASSERT(len > 0 && firstBad >= 0 && numBad < len && firstBad+numBad <= len); if(numBad >= len) return; //should never have been called! if (firstBad == 0) { // The algorithm below has a weird asymmetry in that it // performs poorly when interpolating to the left. If // we're asked to interpolate the left side of a buffer, // we just reverse the problem and try it that way. float *buffer2 = new float[len]; for(i=0; i<len; i++) buffer2[len-1-i] = buffer[i]; InterpolateAudio(buffer2, len, len-numBad, numBad); for(i=0; i<len; i++) buffer[len-1-i] = buffer2[i]; return; } Vector s(len, buffer); // Choose P, the order of the autoregression equation int P = imin(numBad * 3, 50); P = imin(P, imax(firstBad - 1, len - (firstBad + numBad) - 1)); if (P < 3) { LinearInterpolateAudio(buffer, len, firstBad, numBad); return; } // Add a tiny amount of random noise to the input signal - // this sounds like a bad idea, but the amount we're adding // is only about 1 bit in 16-bit audio, and it's an extremely // effective way to avoid nearly-singular matrices. If users // run it more than once they get slightly different results; // this is sometimes even advantageous. for(i=0; i<N; i++) s[i] += (rand()-(RAND_MAX/2))/(RAND_MAX*10000.0); // Solve for the best autoregression coefficients // using a least-squares fit to all of the non-bad // data we have in the buffer Matrix X(P, P); Vector b(P); for(i=0; i<len-P; i++) if (i+P < firstBad || i >= (firstBad + numBad)) for(row=0; row<P; row++) { for(col=0; col<P; col++) X[row][col] += (s[i+row] * s[i+col]); b[row] += s[i+P] * s[i+row]; } Matrix Xinv(P, P); if (!InvertMatrix(X, Xinv)) { // The matrix is singular! Fall back on linear... // In practice I have never seen this happen if // we add the tiny bit of random noise. LinearInterpolateAudio(buffer, len, firstBad, numBad); return; } // This vector now contains the autoregression coefficients Vector a = Xinv * b; // Create a matrix (a "Toeplitz" matrix, as it turns out) // which encodes the autoregressive relationship between // elements of the sequence. Matrix A(N-P, N); for(row=0; row<N-P; row++) { for(col=0; col<P; col++) A[row][row+col] = -a[col]; A[row][row+P] = 1; } // Split both the Toeplitz matrix and the signal into // two pieces. Note that this code could be made to // work even in the case where the "bad" samples are // not contiguous, but currently it assumes they are. // "u" is for unknown (bad) // "k" is for known (good) Matrix Au = MatrixSubset(A, 0, N-P, firstBad, numBad); Matrix A_left = MatrixSubset(A, 0, N-P, 0, firstBad); Matrix A_right = MatrixSubset(A, 0, N-P, firstBad+numBad, N-(firstBad+numBad)); Matrix Ak = MatrixConcatenateCols(A_left, A_right); Vector s_left = VectorSubset(s, 0, firstBad); Vector s_right = VectorSubset(s, firstBad+numBad, N-(firstBad+numBad)); Vector sk = VectorConcatenate(s_left, s_right); // Do some linear algebra to find the best possible // values that fill in the "bad" area Matrix AuT = TransposeMatrix(Au); Matrix X1 = MatrixMultiply(AuT, Au); Matrix X2(X1.Rows(), X1.Cols()); if (!InvertMatrix(X1, X2)) { // The matrix is singular! Fall back on linear... LinearInterpolateAudio(buffer, len, firstBad, numBad); return; } Matrix X2b = X2 * -1.0; Matrix X3 = MatrixMultiply(X2b, AuT); Matrix X4 = MatrixMultiply(X3, Ak); // This vector contains our best guess as to the // unknown values Vector su = X4 * sk; // Put the results into the return buffer for(i=0; i<numBad; i++) buffer[firstBad+i] = (float)su[i]; }
int main(int argc, char* argv[]) { cl_int cl_error; /** Init **/ unsigned int matrix_size = MATRIX_SIZE; unsigned int matrix_total_size = matrix_size*matrix_size; size_t cl_buff_size = matrix_total_size * sizeof(MATRIX_TYPE); printf("Matrix size : %d (%d length)\t |\t sous bloc de %d\n",matrix_size,matrix_total_size,LOCAL_DIM_KERNEL); MATRIX_TYPE * matA = malloc(matrix_total_size * sizeof(*matA)); MATRIX_TYPE * matB = malloc(matrix_total_size * sizeof(*matB)); MATRIX_TYPE * matC = malloc(matrix_total_size * sizeof(*matC)); MATRIX_TYPE * matD = malloc(matrix_total_size * sizeof(*matD)); // Init matA InitMatrix2(matA, matrix_size); // InitMatrix(matB, matrix_size); printf("Matrix A:\n"); DisplayMatrix(matA,matrix_size); // Init GPU cl_uint nb_platf; clGetPlatformIDs(0, NULL, &nb_platf); printf("Nombre de plateformes: %d\t", nb_platf); cl_platform_id platfs[nb_platf]; clGetPlatformIDs(nb_platf, platfs, NULL); size_t plat_name_size; clGetPlatformInfo(platfs[0], CL_PLATFORM_NAME, 0, NULL, &plat_name_size); char plat_name[plat_name_size]; clGetPlatformInfo(platfs[0], CL_PLATFORM_NAME, plat_name_size, &plat_name, NULL); printf("( %s ", plat_name); size_t plat_vendor_size; clGetPlatformInfo(platfs[0], CL_PLATFORM_VENDOR, 0, NULL, &plat_vendor_size); char plat_vendor[plat_vendor_size]; clGetPlatformInfo(platfs[0], CL_PLATFORM_VENDOR, plat_vendor_size, &plat_vendor, NULL); printf("| %s)\n", plat_vendor); cl_uint nb_devs; clGetDeviceIDs(platfs[0], CL_DEVICE_TYPE_ALL, 0, NULL, &nb_devs); cl_device_id devs[nb_devs]; clGetDeviceIDs(platfs[0], CL_DEVICE_TYPE_ALL, nb_devs, devs, NULL); cl_context ctx = clCreateContext(NULL, nb_devs, devs, NULL, NULL, NULL); cl_command_queue command_queue = clCreateCommandQueue(ctx, devs[0], CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_PROFILING_ENABLE, NULL); /** Chargement des kernels **/ cl_kernel ker1, ker2, ker3; bool verbose = false; CreateKernel(&ker1, ctx, devs, nb_devs, "./cholesky_kernel1.cl", "cholesky_diag", verbose); CreateKernel(&ker2, ctx, devs, nb_devs, "./cholesky_kernel2.cl", "cholesky_inf", verbose); CreateKernel(&ker3, ctx, devs, nb_devs, "./cholesky_kernel3.cl", "cholesky_subdiag", verbose); int nombre_de_kernel = 3; cl_event ev_ker[nombre_de_kernel], ev_readA; // Création/Préparation du buffer GPU cl_mem bufA = clCreateBuffer(ctx, CL_MEM_READ_WRITE, cl_buff_size, NULL, &cl_error); CHECK_ERROR(cl_error,"clCreateBuffer"); clEnqueueWriteBuffer(command_queue, bufA, CL_TRUE, 0, cl_buff_size, matA, 0, NULL, &ev_ker[nombre_de_kernel-1]); // ev_ker[n-1] pour préparer la boucle. size_t globalDim[] = {matrix_size, matrix_size}; size_t localDim[] = {LOCAL_DIM_KERNEL, LOCAL_DIM_KERNEL}; // GPU Calculs int i; for (i=0 ; i<matrix_size/LOCAL_DIM_KERNEL/**/ ; i++) { // Kernel 1 : Bloc diagonal clSetKernelArg(ker1, 0, sizeof(bufA), &bufA); clSetKernelArg(ker1, 1, sizeof(i), &i); clEnqueueNDRangeKernel(command_queue, ker1, 2, NULL, globalDim, localDim, 1, &ev_ker[nombre_de_kernel-1], &ev_ker[0]); // Kernel 2 : Blocs sous-diagonaux inferieur clSetKernelArg(ker2, 0, sizeof(bufA), &bufA); clSetKernelArg(ker2, 1, sizeof(i), &i); clEnqueueNDRangeKernel(command_queue, ker2, 2, NULL, globalDim, localDim, 1, &ev_ker[0], &ev_ker[1]); // Kernel 3 : Blocs sous-diagonaux clSetKernelArg(ker3, 0, sizeof(bufA), &bufA); clSetKernelArg(ker3, 1, sizeof(i), &i); clEnqueueNDRangeKernel(command_queue, ker3, 2, NULL, globalDim, localDim, 1, &ev_ker[1], &ev_ker[2]); } clEnqueueReadBuffer(command_queue, bufA, CL_TRUE, 0, cl_buff_size, matB, 1, &ev_ker[nombre_de_kernel-1], &ev_readA); clFinish(command_queue); // clGetEvenProfilingInfo clReleaseMemObject(bufA); /********************/ /*** CHECK RESULT ***/ /********************/ // ClearUpMatrix(matB,matrix_size); printf("\nMatrix B:\n"); DisplayMatrix(matB,matrix_size); /**/ ClearUpMatrix(matB,matrix_size); TransposeMatrix(matB,matC,matrix_size); MullMatrix(matB,matC,matD,matrix_size); MinusMatrix(matD,matA,matrix_size); printf("\nMatrix D - A:\n"); DisplayMatrix(matD,matrix_size); //*/ // Free free(matA); free(matB); free(matC); free(matD); return 0; }
void QClipMap46::TransformedBoxTraceQ3( q3trace_t* Results, const vec3_t Start, const vec3_t End, const vec3_t Mins, const vec3_t Maxs, clipHandle_t Model, int BrushMask, const vec3_t Origin, const vec3_t Angles, int Capsule ) { if ( !Mins ) { Mins = oldvec3_origin; } if ( !Maxs ) { Maxs = oldvec3_origin; } // adjust so that mins and maxs are always symetric, which // avoids some complications with plane expanding of rotated // bmodels vec3_t offset; vec3_t symetricSize[ 2 ]; vec3_t start_l, end_l; for ( int i = 0; i < 3; i++ ) { offset[ i ] = ( Mins[ i ] + Maxs[ i ] ) * 0.5; symetricSize[ 0 ][ i ] = Mins[ i ] - offset[ i ]; symetricSize[ 1 ][ i ] = Maxs[ i ] - offset[ i ]; start_l[ i ] = Start[ i ] + offset[ i ]; end_l[ i ] = End[ i ] + offset[ i ]; } // subtract origin offset VectorSubtract( start_l, Origin, start_l ); VectorSubtract( end_l, Origin, end_l ); // rotate start and end into the models frame of reference bool rotated; if ( Model != BOX_MODEL_HANDLE && ( Angles[ 0 ] || Angles[ 1 ] || Angles[ 2 ] ) ) { rotated = true; } else { rotated = false; } float halfwidth = symetricSize[ 1 ][ 0 ]; float halfheight = symetricSize[ 1 ][ 2 ]; sphere_t sphere; sphere.use = Capsule; sphere.radius = ( halfwidth > halfheight ) ? halfheight : halfwidth; sphere.halfheight = halfheight; float t = halfheight - sphere.radius; vec3_t matrix[ 3 ]; if ( rotated ) { // rotation on trace line (start-end) instead of rotating the bmodel // NOTE: This is still incorrect for bounding boxes because the actual bounding // box that is swept through the model is not rotated. We cannot rotate // the bounding box or the bmodel because that would make all the brush // bevels invalid. // However this is correct for capsules since a capsule itself is rotated too. AnglesToAxis( Angles, matrix ); RotatePoint( start_l, matrix ); RotatePoint( end_l, matrix ); // rotated sphere offset for capsule sphere.offset[ 0 ] = matrix[ 0 ][ 2 ] * t; sphere.offset[ 1 ] = -matrix[ 1 ][ 2 ] * t; sphere.offset[ 2 ] = matrix[ 2 ][ 2 ] * t; } else { VectorSet( sphere.offset, 0, 0, t ); } // sweep the box through the model q3trace_t trace; Trace( &trace, start_l, end_l, symetricSize[ 0 ], symetricSize[ 1 ], Model, Origin, BrushMask, Capsule, &sphere ); // if the bmodel was rotated and there was a collision if ( rotated && trace.fraction != 1.0 ) { // rotation of bmodel collision plane vec3_t transpose[ 3 ]; TransposeMatrix( matrix, transpose ); RotatePoint( trace.plane.normal, transpose ); } // re-calculate the end position of the trace because the trace.endpos // calculated by CM_Trace could be rotated and have an offset trace.endpos[ 0 ] = Start[ 0 ] + trace.fraction * ( End[ 0 ] - Start[ 0 ] ); trace.endpos[ 1 ] = Start[ 1 ] + trace.fraction * ( End[ 1 ] - Start[ 1 ] ); trace.endpos[ 2 ] = Start[ 2 ] + trace.fraction * ( End[ 2 ] - Start[ 2 ] ); *Results = trace; }