int main (int argc, char **argv) {
int N;
double * values;
char type = argc > 2 ? argv[2][0] : 'A';
if (argc < 2) {
fprintf (stderr, "Usage: csort SIZE [ TYPE [SEED]] \n");
exit (1);
}
N = atoi (argv[1]);
srand (argc > 3 ? atol (argv[3]) : 42);
if (type == 'A')
values = randomVector(N, 0.0, 1.0);
else
values = randomVector(N, 1.0, 20.0);
startTiming();
sort (values, N);
stopTiming();
if (N < 20)
printArr(values, N);
return 0;
}
RandomSearch::Candidate RandomSearch::search(const std::vector<std::pair<float, float>>& searchSpace, const int kIterLimit)
{
srand((unsigned)time(0));
RandomSearch::Candidate best;
for (int i = 0; i < kIterLimit; ++i)
{
RandomSearch::Candidate candidate;
candidate.values = randomVector(searchSpace);
candidate.cost = objectiveFunction(candidate.values);
if (!i || candidate.cost < best.cost)
{
best.cost = candidate.cost;
best.values.swap(candidate.values);
}
}
return best;
}
void runTest() {
std::vector<G1> encoded_terms;
randomVector(encoded_terms, m_numberElems);
const PPZK_QueryIC<PAIRING> A(G1::random(),
encoded_terms);
std::stringstream oss;
A.marshal_out_raw(oss);
PPZK_QueryIC<PAIRING> B;
std::stringstream iss(oss.str());
checkPass(B.marshal_in_raw(iss));
checkPass(A == B);
}
void main ()
{
int numValues = 100000;
srandom (17);
apvector<int> vector = randomVector (numValues);
apvector<int> histogram (10, 0);
for (int i = 0; i<numValues; i++) {
int index = vector[i];
histogram[index]++;
}
for (int i = 0; i<10; i++) {
cout << i << "\t" << histogram[i] << endl;
}
}
static void
check_quaternion(generator_t &gen, distribution_t &dist)
{
// check identity
{
quat q;
glm::quat g;
quatIdentity(&q);
assert(q == g);
}
for(size_t x = 0; x < 10000; ++x)
{
// check negation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNegate(q) == -g);
}
// check addition
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatAdd(q1, q2) == g1+g2);
}
// check subtraction
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatSubtract(q1, q2) == g1 + (-g2));
}
// check scale
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float f = dist(gen);
assert(quatScale(q, f) == g*f);
}
// check normalize
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNormalize(q) == glm::normalize(g));
}
// check dot
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(std::abs(quatDot(q1, q2) - glm::dot(g1, g2)) < 0.0001f);
}
// check conjugate
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatConjugate(q) == glm::conjugate(g));
}
// check inverse
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatInverse(q) == glm::inverse(g));
}
// check quaternion multiplication
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatMultiply(q1, q2) == g1*g2);
}
// check vector multiplication
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
assert(quatMultiplyVec3f(q, (vec3f){ v.x, v.y, v.z }) == g*v);
}
// check rotation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
float r = randomAngle(gen, dist);
assert(quatRotate(q, (vec3f){ v.x, v.y, v.z }, r) == glm::rotate(g, r, v));
}
// check rotate X
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateX(q, r) == glm::rotate(g, r, x_axis));
}
// check rotate Y
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateY(q, r) == glm::rotate(g, r, y_axis));
}
// check rotate Z
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateZ(q, r) == glm::rotate(g, r, z_axis));
}
// check conversion from matrix
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
mtx44 m;
quatToMtx44(&m, q);
assert(m == glm::mat4_cast(g));
}
}
}
static void
check_matrix(generator_t &gen, distribution_t &dist)
{
// check identity
{
mtx44 m;
mtx44Identity(&m);
assert(m == glm::mat4());
}
for(size_t x = 0; x < 10000; ++x)
{
// check multiply
{
mtx44 m1, m2;
randomMatrix(m1, gen, dist);
randomMatrix(m2, gen, dist);
glm::mat4 g1 = loadMatrix(m1);
glm::mat4 g2 = loadMatrix(m2);
mtx44 result;
mtx44Multiply(&result, &m1, &m2);
assert(result == g1*g2);
}
// check translate
{
mtx44 m;
randomMatrix(m, gen, dist);
glm::mat4 g = loadMatrix(m);
glm::vec3 v = randomVector(gen, dist);
mtx44Translate(&m, v.x, v.y, v.z);
assert(m == glm::translate(g, v));
}
// check scale
{
mtx44 m;
randomMatrix(m, gen, dist);
glm::mat4 g = loadMatrix(m);
glm::vec3 v = randomVector(gen, dist);
mtx44Scale(&m, v.x, v.y, v.z);
assert(m == glm::scale(g, v));
}
// check rotate
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
glm::vec3 v = randomVector(gen, dist);
mtx44Rotate(&m, (vec3f){ v.x, v.y, v.z }, r);
assert(m == glm::rotate(g, r, v));
}
// check rotate X
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
mtx44RotateX(&m, r);
assert(m == glm::rotate(g, r, x_axis));
}
// check rotate Y
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
mtx44RotateY(&m, r);
assert(m == glm::rotate(g, r, y_axis));
}
// check rotate Z
{
mtx44 m;
randomMatrix(m, gen, dist);
float r = randomAngle(gen, dist);
glm::mat4 g = loadMatrix(m);
mtx44RotateZ(&m, r);
assert(m == glm::rotate(g, r, z_axis));
}
}
}
bool CadScene::loadCSF( const char* filename, int clones, int cloneaxis)
{
CSFile* csf;
CSFileMemoryPTR mem = CSFileMemory_new();
if (CSFile_loadExt(&csf,filename,mem) != CADSCENEFILE_NOERROR || !(csf->fileFlags & CADSCENEFILE_FLAG_UNIQUENODES)){
CSFileMemory_delete(mem);
return false;
}
int copies = clones + 1;
CSFile_transform(csf);
srand(234525);
// materials
m_materials.resize( csf->numMaterials );
for (int n = 0; n < csf->numMaterials; n++ )
{
CSFMaterial* csfmaterial = &csf->materials[n];
Material& material = m_materials[n];
for (int i = 0; i < 2; i++){
material.sides[i].ambient = randomVector(0.0f,0.1f);
material.sides[i].diffuse = nv_math::vec4f(csf->materials[n].color) + randomVector(0.0f,0.07f);
material.sides[i].specular = randomVector(0.25f,0.55f);
material.sides[i].emissive = randomVector(0.0f,0.05f);
}
}
glGenBuffers(1,&m_materialsGL);
glNamedBufferStorageEXT(m_materialsGL, sizeof(Material) * m_materials.size(), &m_materials[0], 0);
//glMapNamedBufferRange(m_materialsGL, 0, sizeof(Material) * m_materials.size(), GL_MAP_PERSISTENT_BIT | GL_MAP_WRITE_BIT);
// geometry
int numGeoms = csf->numGeometries;
m_geometry.resize( csf->numGeometries * copies );
m_geometryBboxes.resize( csf->numGeometries * copies );
for (int n = 0; n < csf->numGeometries; n++ )
{
CSFGeometry* csfgeom = &csf->geometries[n];
Geometry& geom = m_geometry[n];
geom.cloneIdx = -1;
geom.numVertices = csfgeom->numVertices;
geom.numIndexSolid = csfgeom->numIndexSolid;
geom.numIndexWire = csfgeom->numIndexWire;
std::vector<Vertex> vertices( csfgeom->numVertices );
for (int i = 0; i < csfgeom->numVertices; i++){
vertices[i].position[0] = csfgeom->vertex[3*i + 0];
vertices[i].position[1] = csfgeom->vertex[3*i + 1];
vertices[i].position[2] = csfgeom->vertex[3*i + 2];
vertices[i].position[3] = 1.0f;
if (csfgeom->normal){
vertices[i].normal[0] = csfgeom->normal[3*i + 0];
vertices[i].normal[1] = csfgeom->normal[3*i + 1];
vertices[i].normal[2] = csfgeom->normal[3*i + 2];
vertices[i].normal[3] = 0.0f;
}
else{
vertices[i].normal = normalize(nv_math::vec3f(vertices[i].position));
}
m_geometryBboxes[n].merge( vertices[i].position );
}
geom.vboSize = sizeof(Vertex) * vertices.size();
glGenBuffers(1,&geom.vboGL);
glNamedBufferStorageEXT(geom.vboGL,geom.vboSize, &vertices[0], 0);
std::vector<GLuint> indices(csfgeom->numIndexSolid + csfgeom->numIndexWire);
memcpy(&indices[0],csfgeom->indexSolid, sizeof(GLuint) * csfgeom->numIndexSolid);
if (csfgeom->indexWire){
memcpy(&indices[csfgeom->numIndexSolid],csfgeom->indexWire, sizeof(GLuint) * csfgeom->numIndexWire);
}
geom.iboSize = sizeof(GLuint) * indices.size();
glGenBuffers(1,&geom.iboGL);
glNamedBufferStorageEXT(geom.iboGL, geom.iboSize, &indices[0], 0);
if (GLEW_NV_vertex_buffer_unified_memory){
glGetNamedBufferParameterui64vNV(geom.vboGL, GL_BUFFER_GPU_ADDRESS_NV, &geom.vboADDR);
glMakeNamedBufferResidentNV(geom.vboGL, GL_READ_ONLY);
glGetNamedBufferParameterui64vNV(geom.iboGL, GL_BUFFER_GPU_ADDRESS_NV, &geom.iboADDR);
glMakeNamedBufferResidentNV(geom.iboGL, GL_READ_ONLY);
}
geom.parts.resize( csfgeom->numParts );
size_t offsetSolid = 0;
size_t offsetWire = csfgeom->numIndexSolid * sizeof(GLuint);
for (int i = 0; i < csfgeom->numParts; i++){
geom.parts[i].indexWire.count = csfgeom->parts[i].indexWire;
geom.parts[i].indexSolid.count = csfgeom->parts[i].indexSolid;
geom.parts[i].indexWire.offset = offsetWire;
geom.parts[i].indexSolid.offset = offsetSolid;
offsetWire += csfgeom->parts[i].indexWire * sizeof(GLuint);
offsetSolid += csfgeom->parts[i].indexSolid * sizeof(GLuint);
}
}
for (int c = 1; c <= clones; c++){
for (int n = 0; n < numGeoms; n++ )
{
m_geometryBboxes[n + numGeoms * c] = m_geometryBboxes[n];
const Geometry& geomorig = m_geometry[n];
Geometry& geom = m_geometry[n + numGeoms * c];
geom = geomorig;
#if 1
geom.cloneIdx = n;
#else
geom.cloneIdx = -1;
glGenBuffers(1,&geom.vboGL);
glNamedBufferStorageEXT(geom.vboGL,geom.vboSize, 0, 0);
glGenBuffers(1,&geom.iboGL);
glNamedBufferStorageEXT(geom.iboGL,geom.iboSize, 0, 0);
if (GLEW_NV_vertex_buffer_unified_memory){
glGetNamedBufferParameterui64vNV(geom.vboGL, GL_BUFFER_GPU_ADDRESS_NV, &geom.vboADDR);
glMakeNamedBufferResidentNV(geom.vboGL, GL_READ_ONLY);
glGetNamedBufferParameterui64vNV(geom.iboGL, GL_BUFFER_GPU_ADDRESS_NV, &geom.iboADDR);
glMakeNamedBufferResidentNV(geom.iboGL, GL_READ_ONLY);
}
glNamedCopyBufferSubDataEXT(geomorig.vboGL, geom.vboGL, 0, 0, geom.vboSize);
glNamedCopyBufferSubDataEXT(geomorig.iboGL, geom.iboGL, 0, 0, geom.iboSize);
#endif
}
}
glGenBuffers(1,&m_geometryBboxesGL);
glNamedBufferStorageEXT(m_geometryBboxesGL,sizeof(BBox) * m_geometryBboxes.size(), &m_geometryBboxes[0], 0);
glGenTextures(1, &m_geometryBboxesTexGL);
glTextureBufferEXT(m_geometryBboxesTexGL, GL_TEXTURE_BUFFER, GL_RGBA32F, m_geometryBboxesGL);
// nodes
int numObjects = 0;
m_matrices.resize( csf->numNodes * copies );
for (int n = 0; n < csf->numNodes; n++){
CSFNode* csfnode = &csf->nodes[n];
memcpy( m_matrices[n].objectMatrix.get_value(), csfnode->objectTM, sizeof(float)*16 );
memcpy( m_matrices[n].worldMatrix.get_value(), csfnode->worldTM, sizeof(float)*16 );
m_matrices[n].objectMatrixIT = nv_math::transpose( nv_math::invert(m_matrices[n].objectMatrix) );
m_matrices[n].worldMatrixIT = nv_math::transpose( nv_math::invert(m_matrices[n].worldMatrix) );
if (csfnode->geometryIDX < 0) continue;
numObjects++;
}
// objects
m_objects.resize( numObjects * copies );
m_objectAssigns.resize( numObjects * copies );
numObjects = 0;
for (int n = 0; n < csf->numNodes; n++){
CSFNode* csfnode = &csf->nodes[n];
if (csfnode->geometryIDX < 0) continue;
Object& object = m_objects[numObjects];
object.matrixIndex = n;
object.geometryIndex = csfnode->geometryIDX;
m_objectAssigns[numObjects] = nv_math::vec2i( object.matrixIndex, object.geometryIndex );
object.parts.resize( csfnode->numParts );
for (int i = 0; i < csfnode->numParts; i++){
object.parts[i].active = 1;
object.parts[i].matrixIndex = csfnode->parts[i].nodeIDX < 0 ? object.matrixIndex : csfnode->parts[i].nodeIDX;
object.parts[i].materialIndex = csfnode->parts[i].materialIDX;
}
BBox bbox = m_geometryBboxes[object.geometryIndex].transformed( m_matrices[n].worldMatrix );
m_bbox.merge( bbox );
updateObjectDrawCache(object);
numObjects++;
}
// compute clone move delta based on m_bbox;
nv_math::vec4f dim = m_bbox.max - m_bbox.min;
int sq = 1;
int numAxis = 0;
for (int i = 0; i < 3; i++){
numAxis += (cloneaxis & (1<<i)) ? 1 : 0;
}
assert(numAxis);
switch (numAxis)
{
case 1:
sq = copies;
break;
case 2:
while (sq * sq < copies){
sq++;
}
break;
case 3:
while (sq * sq * sq < copies){
sq++;
}
break;
}
for (int c = 1; c <= clones; c++){
int numNodes = csf->numNodes;
nv_math::vec4f shift = dim * 1.05f;
float u = 0;
float v = 0;
float w = 0;
switch (numAxis)
{
case 1:
u = float(c);
break;
case 2:
u = float(c % sq);
v = float(c / sq);
break;
case 3:
u = float(c % sq);
v = float((c / sq) % sq);
w = float( c / (sq*sq));
break;
}
float use = u;
if (cloneaxis & (1<<0)){
shift.x *= -use;
if (numAxis > 1 ) use = v;
}
else {
shift.x = 0;
}
if (cloneaxis & (1<<1)){
shift.y *= use;
if (numAxis > 2 ) use = w;
else if (numAxis > 1 ) use = v;
}
else {
shift.y = 0;
}
if (cloneaxis & (1<<2)){
shift.z *= -use;
}
else {
shift.z = 0;
}
shift.w = 0;
// move all world matrices
for (int n = 0; n < numNodes; n++ )
{
MatrixNode &node = m_matrices[n + numNodes * c];
MatrixNode &nodeOrig = m_matrices[n];
node = nodeOrig;
node.worldMatrix.set_col(3,node.worldMatrix.col(3) + shift);
node.worldMatrixIT = nv_math::transpose( nv_math::invert(node.worldMatrix) );
}
{
// patch object matrix of root
MatrixNode &node = m_matrices[csf->rootIDX + numNodes * c];
node.objectMatrix.set_col(3,node.objectMatrix.col(3) + shift);
node.objectMatrixIT = nv_math::transpose( nv_math::invert(node.objectMatrix) );
}
// clone objects
for (int n = 0; n < numObjects; n++ )
{
const Object& objectorig = m_objects[n];
Object& object = m_objects[ n + numObjects * c];
object = objectorig;
object.geometryIndex += c * numGeoms;
object.matrixIndex += c * numNodes;
for (size_t i = 0; i < object.parts.size(); i++){
object.parts[i].matrixIndex += c * numNodes;
}
for (size_t i = 0; i < object.cacheSolid.state.size(); i++){
object.cacheSolid.state[i].matrixIndex += c * numNodes;
}
for (size_t i = 0; i < object.cacheWire.state.size(); i++){
object.cacheWire.state[i].matrixIndex += c * numNodes;
}
m_objectAssigns[n + numObjects * c] = nv_math::vec2i( object.matrixIndex, object.geometryIndex );
}
}
glGenBuffers(1,&m_matricesGL);
glNamedBufferStorageEXT(m_matricesGL, sizeof(MatrixNode) * m_matrices.size(), &m_matrices[0], 0);
//glMapNamedBufferRange(m_matricesGL, 0, sizeof(MatrixNode) * m_matrices.size(), GL_MAP_PERSISTENT_BIT | GL_MAP_WRITE_BIT);
glGenTextures(1,&m_matricesTexGL);
glTextureBufferEXT(m_matricesTexGL, GL_TEXTURE_BUFFER, GL_RGBA32F, m_matricesGL);
glGenBuffers(1,&m_objectAssignsGL);
glNamedBufferStorageEXT(m_objectAssignsGL,sizeof(nv_math::vec2i) * m_objectAssigns.size(), &m_objectAssigns[0], 0);
if (GLEW_NV_vertex_buffer_unified_memory){
glGetNamedBufferParameterui64vNV(m_materialsGL, GL_BUFFER_GPU_ADDRESS_NV, &m_materialsADDR);
glMakeNamedBufferResidentNV(m_materialsGL, GL_READ_ONLY);
glGetNamedBufferParameterui64vNV(m_matricesGL, GL_BUFFER_GPU_ADDRESS_NV, &m_matricesADDR);
glMakeNamedBufferResidentNV(m_matricesGL, GL_READ_ONLY);
if (GLEW_NV_bindless_texture){
m_matricesTexGLADDR = glGetTextureHandleNV(m_matricesTexGL);
glMakeTextureHandleResidentNV(m_matricesTexGLADDR);
}
}
m_nodeTree.create(copies * csf->numNodes);
for (int i = 0; i < copies; i++){
int cloneoffset = (csf->numNodes) * i;
int root = csf->rootIDX+cloneoffset;
recursiveHierarchy(m_nodeTree,csf,csf->rootIDX,cloneoffset);
m_nodeTree.setNodeParent( (NodeTree::nodeID)root, m_nodeTree.getTreeRoot() );
m_nodeTree.addToTree( (NodeTree::nodeID)root );
}
glGenBuffers(1,&m_parentIDsGL);
glNamedBufferStorageEXT(m_parentIDsGL, m_nodeTree.getTreeCompactNodes().size() * sizeof(GLuint), &m_nodeTree.getTreeCompactNodes()[0], 0);
glGenBuffers(1,&m_matricesOrigGL);
glNamedBufferStorageEXT(m_matricesOrigGL, sizeof(MatrixNode) * m_matrices.size(), &m_matrices[0], 0);
glGenTextures(1,&m_matricesOrigTexGL);
glTextureBufferEXT(m_matricesOrigTexGL, GL_TEXTURE_BUFFER, GL_RGBA32F, m_matricesOrigGL);
CSFileMemory_delete(mem);
return true;
}
Point3 ParticleSystem::initialPos() const
{
return m_origin + memberMult(randomVector(), m_posDeviation);
}
Vector3 ParticleSystem::initialVelocity() const
{
Vector3 vd = memberMult(randomVector(), m_velocityDeviation);
return (m_basicVel + vd) * m_initialVelMag.random();
}
void CDataOut::Disp_out(IplImage *framecopy)
{
char ndisp[100];
sprintf(ndisp,(resdir+"disp%d.txt").c_str(),iter++);
DispFile.open(ndisp);
int ii=0;
CvMat *temp,*temp2,*temp3;
temp=cvCreateMatHeader(6,6,CV_32FC1);
temp2=cvCreateMat(3,6,CV_32FC1);
temp3=cvCreateMat(3,3,CV_32FC1);
CvMat* vect=cvCreateMat (6,1,CV_32FC1);
CvMat* res6=cvCreateMat (6,1,CV_32FC1);
CvMat* vect2=cvCreateMat(1,6,CV_32FC1);
CvMat* proj = cvCreateMat(4,2,CV_32FC1); ///NOT RELEASED
CvMat* m = cvCreateMat(3,1,CV_32FC1);
for ( list<CElempunto*>::iterator It=pEstimator->pMap->bbdd.begin();
It != pEstimator->pMap->bbdd.end(); It++ )
{
if((*It)->state>2)
{
cvmSet(m,0,0,cos((*It)->theta)*sin((*It)->phi));
cvmSet(m,1,0,-sin((*It)->theta));
cvmSet(m,2,0,cos((*It)->theta)*cos((*It)->phi));
cvNormalize( m, m);
/* float xreal=(*It)->wx +cvmGet(m,0,0)/(*It)->rho;
float yreal=(*It)->wy +cvmGet(m,1,0)/(*It)->rho;
float zreal=(*It)->wz +cvmGet(m,2,0)/(*It)->rho;
*/
CvMat *pCovMat = pEstimator->getCovMat();
if (12+ii*6< pCovMat->width && 12+ii*6< pCovMat->height)
{
cvGetSubRect( pCovMat,temp,cvRect(12+ii*6,12+ii*6,6,6) );
for (int part=0; part<40; part++){
cvmSet(vect,0,0,randomVector(-.005,.005));
cvmSet(vect,1,0,randomVector(-.005,.005));
cvmSet(vect,2,0,randomVector(-.005,.005));
cvmSet(vect,3,0,randomVector(-.005,0.005));
cvmSet(vect,4,0,randomVector(-.005,0.005));
cvmSet(vect,5,0,randomVector(-1,1));
cvMatMul(temp,vect,res6);
cvmSet(m,0,0,cos(cvmGet(res6,3,0)+(*It)->theta)*sin(cvmGet(res6,4,0)+(*It)->phi));
cvmSet(m,1,0,-sin(cvmGet(res6,3,0)+(*It)->theta));
cvmSet(m,2,0,cos(cvmGet(res6,3,0)+(*It)->theta)*cos(cvmGet(res6,4,0)+(*It)->phi));
cvNormalize( m, m);
cvmSet (vect2,0,0,((cvmGet(res6,0,0)+(*It)->wx)+cvmGet(m,0,0)/(cvmGet(res6,5,0)+(*It)->rho)));
cvmSet(vect2,0,1,((cvmGet(res6,1,0)+(*It)->wy)+cvmGet(m,1,0)/(cvmGet(res6,5,0)+(*It)->rho)));
cvmSet(vect2,0,2,((cvmGet(res6,2,0)+(*It)->wz)+cvmGet(m,2,0)/(cvmGet(res6,5,0)+(*It)->rho)));
DispFile<<cvmGet(vect2,0,0)<<" ";
DispFile<<cvmGet(vect2,0,1)<<" ";
DispFile<<cvmGet(vect2,0,2)<<" ";
DispFile<<ii<<endl;
cvmSet(vect2,0,0,cvmGet(res6,0,0)+(*It)->wx);
cvmSet(vect2,0,1,cvmGet(res6,1,0)+(*It)->wy);
cvmSet(vect2,0,2,cvmGet(res6,2,0)+(*It)->wz);
cvmSet(vect2,0,3,cvmGet(res6,3,0)+(*It)->theta);
cvmSet(vect2,0,4,cvmGet(res6,4,0)+(*It)->phi);
cvmSet(vect2,0,5,cvmGet(res6,5,0)+(*It)->rho);
pModelCam->cvProject_1_pto(vect2,proj,NULL,NULL,NULL);
if (framecopy != NULL){
cvCircle (framecopy,cvPoint((int)cvmGet(proj,0,0),(int)cvmGet(proj,0,1)),1,cvScalar(0,0,255),1 );
}
}
}
}
ii++;
}
cvReleaseMatHeader(&temp);
cvReleaseMat(&temp2);
cvReleaseMat(&temp3);
cvReleaseMat(&vect);
cvReleaseMat(&vect2);
cvReleaseMat(&res6);
cvReleaseMat(&proj);
DispFile.close();
}
