/*
return index for a position if it exists, otherwise return -1
*/
int vertIndex(vec3 v){
int i, index=-1;
for(i=0; i<vertListSize; i++){
if(compareVectors(verts[i].position, v) == 1){
index = i;
break;
}
}
return index;
}
int main(){
int r,i,originalVector[VECTORSIZE],sortedVector[VECTORSIZE],testVector[VECTORSIZE];
/*seed random number*/
srand(1234);
clock_t start, diff;
/*initialize random number*/
for (i=0;i<VECTORSIZE;i++)originalVector[i]=rand()%10000;
/*copy vector to sorted vector*/
memmove(sortedVector,originalVector,sizeof(originalVector));
/*insertion sort*/
start=clock(); /*start timer*/
insertion_sort(sortedVector,VECTORSIZE);
printf ("Insertion sort took %d ms\n",(clock()-start)*1000/CLOCKS_PER_SEC);
/*sort1*/
/*copy vector to test vector*/
memmove(testVector,originalVector,sizeof(originalVector));
start=clock(); /*start timer*/
sort1(testVector,VECTORSIZE);
printf ("sort1 took %d ms\n",(clock()-start)*1000/CLOCKS_PER_SEC);
/*check that it is sorted*/
if(!compareVectors(sortedVector,testVector,VECTORSIZE)){
printf("Sort1 failed\n");
return 0;
}
/*sort2*/
/*copy vector to test vector*/
memmove(testVector,originalVector,sizeof(originalVector));
start=clock(); /*start timer*/
sort2(testVector,VECTORSIZE);
printf ("sort2 took %d ms\n",(clock()-start)*1000/CLOCKS_PER_SEC);
/*check that it is sorted*/
if(!compareVectors(sortedVector,testVector,VECTORSIZE)){
printf("Sort2 failed\n");
return 0 ;
}
return 1;
}
/* Function for sorting bodies */
static int compareBodies(const void* _a, const void* _b)
{
const Body* a = (const Body*) _a;
const Body* b = (const Body*) _b;
int rc;
char* bufA;
char* bufB;
if ((rc = compareComponents(Mass(a), Mass(b))))
return rc;
/* Masses equal, compare positions */
rc = compareVectors(Pos(a), Pos(b));
if (rc == 0)
{
bufA = showBody(a);
bufB = showBody(b);
mw_panic("Comparing bodies with equal positions: %s, %s\n", bufA, bufB);
free(bufA); /* Never reached */
free(bufB);
}
return rc;
}
/**
* \param Tuple The first numeric tuple.
* \param Tuple The second numeric tuple.
*
* \return Tuple The first tuple when when that tuple is lexicographical greater than or equal to the other, the second one otherwise.
*
* If one of the tuples is shorter, the remaining of the elements will be interpreted as 0.
*/
void VMAX(Machine & machine){
Tuple b = machine.stack.popTuple();
Tuple a = machine.stack.popTuple();
machine.stack.push(compareVectors(a,b) >= 0 ? a : b);
}
/**
* \param Tuple The first numeric tuple.
* \param Tuple The second numeric tuple.
*
* \return Number 1 when when the first tuple is lexicographical greater than or equal to the second, 0 otherwise.
*
* If one of the tuples is shorter, the remaining of the elements will be interpreted as 0.
*/
void VGTE(Machine & machine){
machine.stack.push(compareVectors(machine) >= 0 ? 1 : 0);
}
/**
* \param Tuple The first numeric tuple.
* \param Tuple The second numeric tuple.
*
* \return Number 1 when when the first tuple is lexicographical less than the second, 0 otherwise.
*
* If one of the tuples is shorter, the remaining of the elements will be interpreted as 0.
*/
void VLT(Machine & machine){
machine.stack.push(compareVectors(machine) < 0 ? 1 : 0);
}
/**
* \param Tuple The first numeric tuple.
* \param Tuple The second numeric tuple.
*
* \return Number 0 when the elements in the tuples are equal, 1 otherwise.
*
* If one of the tuples is shorter, the remaining of the elements will be interpreted as 0.
*/
void VNEQ(Machine & machine){
machine.stack.push(compareVectors(machine) != 0 ? 1 : 0);
}
TEST_F(ExoticaTaskTest, DTT) //!< Derived-task Jacobian Tests: will contain tests to check whether the jacobian is correctly computed through finite-differences method
{
boost::shared_ptr<exotica::TaskDefinition> task_ptr;
for (int i = 0; i < registered_types_.size(); i++) //!< Iterate through the registered tasks
{
std::string xml_path;
tinyxml2::XMLDocument document;
boost::shared_ptr<tinyxml2::XMLHandle> element_ptr;
if (exotica::TestRegistrar::Instance()->findXML(registered_types_[i],
xml_path)) //!< If it is wished to be tested...
{
if (document.LoadFile((resource_path_ + xml_path).c_str())
!= tinyxml2::XML_NO_ERROR) //!< Attempt to load file
{
ADD_FAILURE()<< " : Could not Load initialiser for " << registered_types_[i] << " (file: "<<resource_path_ + xml_path <<")."; //!< Have to use this method to add failure since I do not want to continue for this object but do not wish to abort for all the others...
continue;//!< Go to next object
}
//!< Initialise
if (!(task_ptr = exotica::TaskCreator::Instance()->createObject(registered_types_[i], params_)))//!< If we could not create
{
ADD_FAILURE() << " : Could not create object of type " << registered_types_[i]; //!< Have to use this method to add failure since I do not want to continue for this object but do not wish to abort for all the others...
continue;//!< Go to next object
}
tinyxml2::XMLHandle xml_handle(document.RootElement()); //!< Get a handle to the root element
if (!task_ptr->initBase(xml_handle))
{
ADD_FAILURE() << " : XML Initialiser is malformed for " << registered_types_[i];
continue;
}
//!< Now our testing stuff...
element_ptr.reset(new tinyxml2::XMLHandle(xml_handle.FirstChildElement("TestPoint")));
if (!element_ptr)
{
ADD_FAILURE() << " : XML Tester is malformed for " << registered_types_[i];
continue;
}
int j=0;
while (element_ptr->ToElement()) //!< Iterate through all possible test cases
{
Eigen::VectorXd conf_space; //!< Vector containing the configuration point for testing
Eigen::VectorXd task_space;//!< Vector with the task-space co-ordinate of the forward map
Eigen::VectorXd phi;//!< The computed phi
Eigen::MatrixXd jacobian;//!< The Jacobian computation
if (!element_ptr->FirstChildElement("ConfSpace").ToElement())//!< If inexistent
{
ADD_FAILURE() << " : Missing Config for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
if (!exotica::getVector(*(element_ptr->FirstChildElement("ConfSpace").ToElement()), conf_space))
{
ADD_FAILURE() << " : Could not parse Config Vector for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
if (!element_ptr->FirstChildElement("TaskSpace").ToElement()) //!< If inexistent
{
ADD_FAILURE() << " : Missing Task Space point for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
if (!exotica::getVector(*(element_ptr->FirstChildElement("TaskSpace").ToElement()), task_space))
{
ADD_FAILURE() << " : Could not parse task vector for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue; //!< With the inner while loop...
}
//!< First test forward mapping
if (!task_ptr->updateTask(conf_space, 0))
{
ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue;
}
phi = task_ptr->getPhi(0);
jacobian = task_ptr->getJacobian(0);
if (phi.size() != task_space.size())
{
ADD_FAILURE() << " Task-size mismatch for " << registered_types_[i] << " @ iteration " << j;
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
continue;
}
EXPECT_TRUE(compareVectors(phi, task_space, TOLERANCE)) << " : Phi mismatch for " << registered_types_[i] << " @ iteration " << j;
//!< Now the Jacobian (finite differences method)
for (int k=0; k<conf_space.size(); k++)
{
Eigen::VectorXd conf_perturb = conf_space;
conf_perturb(k) += EPSILON;
if (!task_ptr->updateTask(conf_perturb, 0))
{
ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k;
continue;
}
if (task_ptr->getPhi(0).size() != phi.size())
{
ADD_FAILURE() << " for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k;
continue;
}
Eigen::VectorXd task_diff = (task_ptr->getPhi(0) - phi)/EPSILON; //!< Final - initial
Eigen::VectorXd jac_column = jacobian.col(k);//!< Get the desired column (for the current joint)
EXPECT_TRUE(compareVectors(task_diff, jac_column, TOLERANCE)) << " Incorrect for " << registered_types_[i] << " @ iteration " << j << " in config-dimension " << k << task_diff << "\n" << jac_column;
}
element_ptr.reset(new tinyxml2::XMLHandle(element_ptr->NextSiblingElement("TestPoint")));
j++;
} //!< Iteration through j
} //check on testing
} //!< Iteration through i
}
bool operator() (const std::vector<PIECEID_T>& lhs, const std::vector<PIECEID_T>& rhs) const
{
return compareVectors(lhs, rhs) < 0;
}
