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;
}
