int main(int argc,char **argv){
struct tms s1;
struct tms s2;
volatile int count,count1;
volatile int i;
volatile double X=1024;
Clock *test_clk = CreateClock();
StartClock(test_clk);
for(i=0;i<1000000;i++){
count1+=(i+1);
count1 = (count1*i)/count1;
X += ((X*X) + X/0.331)*(X*X*X);
}
StopClock(test_clk);
printf("PROBE1: Clock Ticks Elapsed is %Lu \n",(clock_t)GetClockTicks(test_clk));
StartClock(test_clk);
for(i=0;i<1000000;i++){
count1+=(i+1);
count1 = (count1*i)/count1;
X += ((X*X) + X/0.331)*(X*X*X);
}
StopClock(test_clk);
printf("PROBE2: Clock Ticks Elapsed is %Lu \n",(clock_t)GetClockTicks(test_clk));
printf("size of clock_t is %d \n",sizeof(clock_t));
}
/*
*takes a binary file, set the columns which you want to
*sort in ESortParams.col_start and ESortParams.col_end
*/
void ExSortKmerEdges(const char *bin_kedge, const char *run_file,
unsigned char KEY_SIZE, ExKeyCompare key_compare){
unsigned char *run_buffer = NULL;
size_t key_buf_len = (KEY_SIZE*KEYS_IN_RUN);
size_t buf_len = key_buf_len + sizeof(unsigned long);
size_t ret_len;
int bin_kedge_fd = open(bin_kedge, O_RDONLY);
int run_file_fd = fileno(fopen(run_file, "w"));
unsigned long *runlen;
unsigned long rcount = 0;
unsigned char *key_buf;
Clock *clk = CreateClock();
if(!(bin_kedge_fd > 0 && run_file_fd > 0)){
perror("FAILED TO OPEN FILES:");
assert(0);
}
run_buffer = malloc(sizeof(unsigned char)*buf_len);
assert(run_buffer);
runlen = (unsigned long *)run_buffer;
key_buf = run_buffer + sizeof(unsigned long);
StartClock(clk);
while((ret_len = SafeRead(bin_kedge_fd, key_buf, key_buf_len)) >= KEY_SIZE){
*runlen = (unsigned long) ret_len;
IntegerSort_SB(key_buf, key_buf + (*runlen) - KEY_SIZE, KEY_SIZE, 1,
ESortParams.col_start, ESortParams.col_end, 0, CharMap,
ESortParams.endian);
ret_len = SafeWrite(run_file_fd, run_buffer,
((*runlen + sizeof(unsigned long))< buf_len)?(*runlen +
sizeof(unsigned long)):buf_len);
rcount++;
assert(ret_len == ((*runlen + sizeof(unsigned long)
< buf_len)?(*runlen + sizeof(unsigned long)):buf_len));
}
StopClock(clk);
FreeISortBuckets();
/*create the final run*/
close(run_file_fd); close(bin_kedge_fd);
free(run_buffer);
printf("\n[EX-SORT CREATED %lu RUNS] took %ld ticks\n", rcount, GetClockTicks(clk));
StartClock(clk);
/*call the external rway merge*/
ExternalRWayMerge(run_file, RUNS_PER_MERGE, KEY_SIZE, key_compare, rcount);
StopClock(clk);
printf("[R-WAY MERGE] took %ld ticks\n", GetClockTicks(clk));
}
int main()
{
StartClock();
unsigned int *no_init_ptr;
unsigned int *ptr;
struct UnsignedInt_Type1 ptr_str;
((ptr_str.Void_Type0::ptr = ptr , ptr_str.addr = (reinterpret_cast < unsigned long long > ((&ptr)))) , ((Assign(&ptr_str,UnsignedInt_Type1_Cast_Void_Type0(malloc_overload(400UL))) , ptr = ptr_str.Void_Type0::ptr)));
unsigned int *ptr2;
struct UnsignedInt_Type1 ptr2_str;
((ptr2_str.Void_Type0::ptr = ptr2 , ptr2_str.addr = (reinterpret_cast < unsigned long long > ((&ptr2)))) , ((Assign(&ptr2_str,UnsignedInt_Type1_Cast_Void_Type0(malloc_overload(40UL))) , ptr2 = ptr2_str.Void_Type0::ptr)));
unsigned int *ptr_index;
unsigned int counter = 0U;
struct UnsignedInt_Type1 UnsignedInt_Type1_ovl_2;
struct UnsignedInt_Type1 UnsignedInt_Type1_ovl_3;
for ((((UnsignedInt_Type1_ovl_2 = create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index)))) , UnsignedInt_Type1_Assign_UnsignedInt_Type1_UnsignedInt_Type1((reinterpret_cast < unsigned long long > ((&UnsignedInt_Type1_ovl_2))),ptr_str)) , ((unsigned int *)( *(reinterpret_cast < void ** > (UnsignedInt_Type1_ovl_2.addr)))) = UnsignedInt_Type1_ovl_2.Void_Type0::ptr) , UnsignedInt_Type1_ovl_2); b_LessThan_UnsignedInt_Type1_UnsignedInt_Type1(create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index)))),UnsignedInt_Type1_Add_UnsignedInt_Type1_i(ptr_str,100)); (((UnsignedInt_Type1_ovl_3 = create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index)))) , UnsignedInt_Type1_Increment_UnsignedInt_Type1((reinterpret_cast < unsigned long long > ((&UnsignedInt_Type1_ovl_3))))) , ((unsigned int *)( *(reinterpret_cast < void ** > (UnsignedInt_Type1_ovl_3.addr)))) = UnsignedInt_Type1_ovl_3.Void_Type0::ptr) , UnsignedInt_Type1_ovl_3)) {
*Deref(create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index))))) = counter++;
}
struct UnsignedInt_Type1 UnsignedInt_Type1_ovl_4;
struct UnsignedInt_Type1 UnsignedInt_Type1_ovl_5;
for ((((UnsignedInt_Type1_ovl_4 = create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index)))) , UnsignedInt_Type1_Assign_UnsignedInt_Type1_UnsignedInt_Type1((reinterpret_cast < unsigned long long > ((&UnsignedInt_Type1_ovl_4))),UnsignedInt_Type1_Sub_UnsignedInt_Type1_i(UnsignedInt_Type1_Add_UnsignedInt_Type1_i(ptr_str,100),1))) , ((unsigned int *)( *(reinterpret_cast < void ** > (UnsignedInt_Type1_ovl_4.addr)))) = UnsignedInt_Type1_ovl_4.Void_Type0::ptr) , UnsignedInt_Type1_ovl_4); b_GreaterOrEqual_UnsignedInt_Type1_UnsignedInt_Type1(create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index)))),ptr_str); (((UnsignedInt_Type1_ovl_5 = create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index)))) , UnsignedInt_Type1_Decrement_UnsignedInt_Type1((reinterpret_cast < unsigned long long > ((&UnsignedInt_Type1_ovl_5))))) , ((unsigned int *)( *(reinterpret_cast < void ** > (UnsignedInt_Type1_ovl_5.addr)))) = UnsignedInt_Type1_ovl_5.Void_Type0::ptr) , UnsignedInt_Type1_ovl_5)) {
printf("%u\n", *Deref(create_struct(ptr_index,(reinterpret_cast < unsigned long long > ((&ptr_index))))));
}
EndClock();
return 1;
}
void LogTransformT(RowStarts *rowptr,ColIndices *colind,ValueType *val,Indices n,ValueType *u,
ValueType *v,ValueType *dist,Indices *m,Indices *m_inv){
ValueType *max_c = new ValueType[n];
rowptr--;colind--;val--;max_c--; m--; m_inv--; u--; dist--;
Indices k,i,j,j0;
Clock *anst_clk;
anst_clk = CreateClock();
StartClock(anst_clk);
/*TODO: Avoid HUGE_VAL directly in templated code*/
for(j=1;j<=n;j++){
/*Find the maximum in the column*/
max_c[j] = (ValueType)0.0;
m[j] = (Indices)0; m_inv[j]=0;
u[j] = HUGE_VAL;dist[j]=HUGE_VAL;
}
for(i=1;i<=n;i++){
for(k=rowptr[i]+1;k<(rowptr[i+1]+1);k++){
val[k] = fabs(val[k]);
j = colind[k]+1;
if(val[k] > max_c[j]){
max_c[j] = val[k];
}
}
}
#if 0
for(k=1;k<=n;k++){
printf("max in col %u is %e \n",k,max_c[k]);
}
#endif
for(i=1;i<=n;i++){
for(k=rowptr[i]+1;k<(rowptr[i+1]+1);k++){
j = colind[k]+1;
if(val[k]<=1.0e-30){
val[k] = HUGE_VAL;
}else{
if(max_c[j]<=1.0e-30){
val[k] = HUGE_VAL;//MAX_DOUBLE_VALUE/n - (log(val[k])/M_LN10);
}else{
val[k] = log10(max_c[j]/val[k]);
val[k] = fabs(val[k]);
if(val[k]< u[j]){
u[j] = val[k];
}
}
}
/*take care of -0.0*/
/*assert(val[k]>=0.0);*/
}
}
StopClock(anst_clk);
anst_ticks = GetClockTicks(anst_clk);
++max_c;
delete max_c;
}
void ANSTMC21(RowStarts rowptr,ColIndices colind,Indices n,Indices *m){
Indices *col_marker;
Clock *anst_clock = CreateClock();
Indices *p; Indices *m_inv;
CreateMC21WorkSpace(n,&col_marker,&p,&m_inv);
StartClock(anst_clock);
FindPerfectMatch(rowptr,colind,n,m,p,m_inv,col_marker);
StopClock(anst_clock);
anst_ticks = GetClockTicks(anst_clock);
FreeMC21WorkSpace(col_marker,p,m_inv);
free(anst_clock);
}
int main()
{
StartClock();
int *ptr = (int *)(malloc((100 * (sizeof(int )))));
int *ptr2 = (int *)(malloc((10 * (sizeof(int )))));
(__builtin_expect((!(ptr2 != 0L)),0))?__assert_rtn(__func__,"/Users/vanka1/research/compilers/rose_public/rose_build/projects/RTC/pointer_example.cpp",17,"ptr2 != NULL") : ((void )0);
int *start_ptr = ptr;
int *start_ptr2 = ptr2;
// Crossing the boundary of ptr. The condition should
// be less than, not less than or equal to
// ptr[PTR_SIZE] is an out-of-bounds access
for (int index = 0; index <= (100 + 1); index++) {
*ptr = index;
ptr++;
}
// Resetting ptr to start_ptr, so that it points to the beginning
// of the allocation
ptr = start_ptr;
// Printing what we wrote above
for (int index = 0; index <= (100 + 1); index++) {
printf("ptr[%d]=%d\n",index, *ptr);
ptr++;
}
#if 0
// Resetting ptr to start_ptr, so that it points to the beginning
// of the allocation
// Memsetting ptr and ptr2 allocations, in one go.
// This is also crossing the boundaries of ptr. It assumes that
// ptr and ptr2 are in contiguous locations
// Resetting ptr to start_ptr, so that it points to the beginning
// of the allocation
// Printing ptr and ptr2 *and* one more beyond ptr2, all using
// ptr! This still works since malloc asks for more than it needs
// always.
#endif
(__builtin_expect((!(ptr2 != 0L)),0))?__assert_rtn(__func__,"/Users/vanka1/research/compilers/rose_public/rose_build/projects/RTC/pointer_example.cpp",68,"ptr2 != NULL") : ((void )0);
printf("Before free ptr2\n");
fflush(0L);
free(ptr2);
#if 0
#if 0
// Retrying the print above, after freeing ptr2. This should
// crash--- and it does!
#endif
// Allocating another pointer
// This allocation might take the place of ptr2. In this case,
// printing ptr beyond its boundaries should be okay
// Nope this also crashes!
#endif
EndClock();
return 1;
}
int main()
{
StartClock();
struct UIntStruct ptr_structed0 = UInt_Void_Cast(malloc_wrap(((size_t )(((unsigned long long )100) * (sizeof(int ))))));
struct UIntStruct ptr2_structed1 = UInt_Void_Cast(malloc_wrap(((size_t )(((unsigned long long )10) * (sizeof(int ))))));
struct UIntStruct start_ptr_structed2 = ptr_structed0;
struct UIntStruct start_ptr2_structed3 = ptr2_structed1;
struct UIntStruct start_ptr3_structed4 = UInt_Void_Cast(malloc_wrap(((size_t )(((unsigned long long )100) * (sizeof(unsigned int ))))));
struct UIntStruct start_ptr4_structed5 = start_ptr2_structed3;
#if 0
#endif
EndClock();
return 1;
}
uint32_t ImproveCommunities(const CGraph* graph, CommunityPartition* partition, uint32_t numThreads, uint32_t lookahead, const double64_t alfa ) {
num_threads = numThreads;
omp_set_num_threads(num_threads);
printf("Maximum number of threads: %d\n", omp_get_max_threads());
printf("Starting improvement from a partition with WCC: %f\n", partition->m_WCC / graph->GetNumNodes());
CommunityPartition bestPartition;
CopyPartition(&bestPartition, partition);
uint32_t remainingTries = lookahead;
bool improve = true;
while(improve) {
while (improve) {
printf("\n");
uint64_t initTime = StartClock();
improve = false;
printf("Starting improvement iteration ...\n");
if (PerformImprovementStep(graph, partition, alfa)) {
printf("Error while performing an improvement step.\n");
return 1;
}
printf("New WCC: %f\n", partition->m_WCC / graph->GetNumNodes());
printf("Best WCC: %f\n", bestPartition.m_WCC / graph->GetNumNodes());
printf("Memory required by this iteration: %lu bytes \n", MeasureMemoryConsumption(partition) + MeasureMemoryConsumption(&bestPartition));
if (partition->m_WCC - bestPartition.m_WCC > 0.0f) {
// if (((partition->m_WCC - bestPartition.m_WCC) / bestPartition.m_WCC) > 0.01f) {
remainingTries = lookahead;
// }
FreeResources(&bestPartition);
CopyPartition(&bestPartition, partition);
}
printf("Iteration time: %lu ms\n", StopClock(initTime));
if(remainingTries > 0) {
remainingTries--;
improve = true;
}
}
}
FreeResources(partition);
CopyPartition(partition, &bestPartition);
FreeResources(&bestPartition);
return 0;
}
int main(int argc, char** argv)
{
Application* app;
bool isController;
if (argc < 2)
STOP_FAIL(1, "Config file argument not given, exiting");
if (!configFile.Init(argv[1]))
STOP_FAIL(1, "Invalid config file (%s)", argv[1]);
InitLog();
ParseArgs(argc, argv);
StartClock();
ConfigureSystemSettings();
IOProcessor::Init(configFile.GetIntValue("io.maxfd", 32768));
InitContextTransport();
BloomFilter::StaticInit();
isController = IsController();
LogPrintVersion(isController);
if (isController)
app = new ConfigServerApp;
else
app = new ShardServerApp;
app->Init();
IOProcessor::BlockSignals(IOPROCESSOR_BLOCK_ALL);
EventLoop::Init();
EventLoop::Run();
Log_Message("Shutting down...");
EventLoop::Shutdown();
app->Shutdown();
delete app;
IOProcessor::Shutdown();
StopClock();
configFile.Shutdown();
Log_Shutdown();
return 0;
}
byte PlayALevel(byte map)
{
int lastTime = 1;
byte exitcode = 0;
if (!InitLevel(map))
{
mapToGoTo = 255;
return LEVEL_ABORT;
}
exitcode = LEVEL_PLAYING;
gameMode = GAMEMODE_PLAY;
CDMessingTime = 0;
garbageTime = 0;
UpdateGuys(curMap, &curWorld); // this will force the camera into the right position
// it also makes everybody animate by one frame, but no one will
// ever notice
while (exitcode == LEVEL_PLAYING)
{
lastTime += TimeLength() - CDMessingTime;
StartClock();
if (gameMode == GAMEMODE_PLAY)
HandleKeyPresses();
exitcode = LunaticRun(&lastTime);
LunaticDraw();
if (lastKey == 27 && gameMode == GAMEMODE_PLAY)
{
InitPauseMenu();
gameMode = GAMEMODE_MENU;
}
if (!gamemgl->Process())
{
exitcode = LEVEL_ABORT;
mapToGoTo = 255;
}
EndClock();
}
ExitLevel();
return exitcode;
}
void LogTransform(SparseGraph *G){
node_t *rowptr = G->rowptr; rowptr--;
node_t *colind = G->colind; colind--;
double *val = G->nnz;val--;
node_t n = G->order; node_t k;
node_t nnz_size = G->nnz_size;
node_t k0,j0;
double *max_c = (double *)calloc(n,sizeof(double));max_c--;
#if 0
for(k=1;k<=n;k++){
/*Find the maximum in the column*/
max_c[k] = (double)0.0;
}
#endif
Clock *anst_clk = CreateClock();
StartClock(anst_clk);
for(k=1;k<=nnz_size;k++){
val[k] = fabs(val[k]);
if(val[k] > max_c[colind[k]]){
max_c[colind[k]] = val[k];
}
}
#if 0
for(k=1;k<=n;k++){
printf("max in col %u is %e \n",k,max_c[k]);
}
#endif
for(k=1;k<=nnz_size;k++){
if(fabs(val[k]-0.0)<=1.0e-30){
val[k] = HUGE_VAL;
}else{
if(fabs(max_c[colind[k]]-0.0)<=1.0e-30){
val[k] = HUGE_VAL;//MAX_DOUBLE_VALUE/n - (log(val[k])/M_LN10);
}else{
val[k] = log10(max_c[colind[k]]/val[k]);
val[k] = fabs(val[k]);
}
}
/*take care of -0.0*/
assert(val[k]>=0.0);
}
StopClock(anst_clk);
anst_ticks = GetClockTicks(anst_clk);
free(++max_c);
}
int main()
{
StartClock();
struct UIntStruct ptr_structed0 = UInt_Void_Cast(malloc_wrap(((size_t )(((unsigned long long )100) * (sizeof(int ))))));
struct UIntStruct ptr2_structed1 = UInt_Void_Cast(malloc_wrap(((size_t )(((unsigned long long )10) * (sizeof(int ))))));
struct UIntStruct start_ptr_structed2 = ptr_structed0;
struct UIntStruct start_ptr2_structed3 = ptr2_structed1;
struct UIntStruct start_ptr3_structed4 = UInt_Void_Cast(malloc_wrap(((size_t )(((unsigned long long )100) * (sizeof(unsigned int ))))));
struct UIntStruct start_ptr4_structed5 = start_ptr2_structed3;
#if 1
*UInt_Deref(start_ptr_structed2) = 1;
*UInt_Deref(start_ptr2_structed3) = 1;
*UInt_Deref(ptr_structed0) = 3;
*UInt_Deref(ptr2_structed1) = 9;
#endif
for (struct UIntStruct new_ptr_structed6 = start_ptr_structed2; UInt_UInt_Struct_LessThan(new_ptr_structed6,UInt_Int_Normal_Add(start_ptr_structed2,100)); new_ptr_structed6 = UInt_UInt_Normal_Add(new_ptr_structed6,1U)) {
*UInt_Deref(new_ptr_structed6) = 5;
printf("%u\n", *UInt_Deref(new_ptr_structed6));
}
EndClock();
return 1;
}
byte WorldPickerPause(void)
{
int lastTime = 1;
byte exitcode = LEVEL_PLAYING;
InitPauseMenu();
SetGiveUpText(2);
while (exitcode == LEVEL_PLAYING)
{
lastTime += TimeLength();
StartClock();
exitcode = WorldPauseRun(&lastTime);
WorldPauseDraw();
if (!gamemgl->Process())
{
exitcode = WORLD_QUITGAME;
mapToGoTo = 255;
}
EndClock();
}
return exitcode;
}
void WorldSQP::solve() {
#if TIMER_ENABLED
StartClock();
#endif
#if COLOCATION_METHOD
assert(current_states.size() == _num_traj);
cout << current_controls.size() << " " << _num_worlds - 1 << endl;
assert(current_controls.size() == _num_worlds-1);
for (int i = 0; i < current_states.size(); i++) {
assert(current_states[i].size() == _num_worlds); // memory leak if false
}
#else
assert(false); //solver not implemented for SHOOTING_METHOD
#endif
//vector to contain the new states
VectorXd new_states(_num_traj*(_num_worlds-2)*_size_each_state + (_num_worlds-1)*_size_each_control);
//setup filenames
char filename_goalvec[256];
sprintf(filename_goalvec, "%s/%s_%s", SQP_BASE_FOLDER, _namestring, FILENAME_GOALVEC);
char filename_alltrans[256];
sprintf(filename_alltrans, "%s/%s_%s", SQP_BASE_FOLDER, _namestring, FILENAME_ALLTRANS);
char filename_initctrls[256];
sprintf(filename_initctrls, "%s/%s_%s", SQP_BASE_FOLDER, _namestring, FILENAME_INIT_CTRLS);
//compute Jacobians
compute_all_jacobians();
double t0 = GetClock();
//write out all Jacobians to file
SparseMatrix<double> all_trans_sparse(_all_trans);
Matrix_To_File(all_trans_sparse, filename_alltrans);
//write out all states to file
VectorXd goal_vector(_size_each_state*_num_worlds*_num_traj);
VectorXd state_for_file(_size_each_state);
for (int j=0; j < current_states.size(); j++) {
for (int i=0; i < current_states[j].size(); i++) {
world_to_state(current_states[j][i], state_for_file);
goal_vector.segment((_num_worlds)*_size_each_state*j + i*_size_each_state, _size_each_state) = state_for_file;
}
}
Vector_To_File(goal_vector, filename_goalvec);
//write out all controls to file
VectorXd init_controls(_size_each_control*(_num_worlds-1));
for (int j = 0; j < current_controls.size(); j++)
{
vector<Control*> cu = current_controls[j];
VectorXd vu;
current_states[0][j]->ControlToVectorXd(cu, vu);
vu = current_states[0][j]->JacobianControlStripper(vu);
assert(vu.size() == _size_each_control);
init_controls.segment(j*_size_each_control, _size_each_control) = vu;
}
Vector_To_File(init_controls, filename_initctrls);
//prepare to execute solver (MATLAB)
char filename_statevec_thisiter[256];
sprintf(filename_statevec_thisiter, "%s/%s_%s%d.txt", SQP_BASE_FOLDER, _namestring, FILENAME_STATEVEC_BASE, 1);
//char matlab_command[1024];
//sprintf(matlab_command, "%s -nodisplay -nodesktop -nojvm -r \"solve_sparse(%d, %d, \'%s\', %d, %d, \'%s\', \'%s\', %d, %d, %d)\"", MATLAB_INSTALL, _all_trans.rows(), _all_trans.cols(), filename_alltrans, goal_vector.rows(), goal_vector.cols(), filename_goalvec, filename_statevec_thisiter, _num_worlds, _size_each_state, _size_each_control);
//char matlab_command[1024];
//sprintf(matlab_command, "java -jar MatlabClient.jar \"solve_sparse(%d, %d, \'%s\', %d, %d, \'%s\', \'%s\', %d, %d, %d)\"", _all_trans.rows(), _all_trans.cols(), filename_alltrans, goal_vector.rows(), goal_vector.cols(), filename_goalvec, filename_statevec_thisiter, _num_worlds, _size_each_state, _size_each_control);
char python_command[1024];
sprintf(python_command, "python ../MotionPlanning/SQPSolver.py solver %d %d \'%s\' %d %d \'%s\' \'%s\' \'%s\' %d %d %d %d %f %f %f %f %f %f %f", _all_trans.rows(), _all_trans.cols(), filename_alltrans, goal_vector.rows(), goal_vector.cols(), filename_goalvec, filename_initctrls, filename_statevec_thisiter, _num_traj, _num_worlds, _size_each_state, _size_each_control, LAMBDA_U, LAMBDA_U_DOT, LAMBDA_DIST_FROM_GOAL, LAMBDA_DIST_FROM_PAIR, STATE_INIT_CONSTRAINT, TRANSL_INIT_CONSTRAINT, ROT_INIT_CONSTRAINT );
std::cout << "command: " << python_command << std::endl;
#if TIMER_ENABLED
double JCT = GetClock();
StartClock();
#endif
//run solver (MATLAB) // NONO! ITS PYTHON/MOSEK!
//int return_value = system(matlab_command);
int return_value = system(python_command);
#if TIMER_ENABLED
double MST = GetClock();
StartClock();
#endif
//Read solver output
File_To_Vector(filename_statevec_thisiter, new_states);
vector<vector<VectorXd> > sqp_intermediate_states;
sqp_intermediate_states.resize(_num_traj);
//copy out new states
for (int j = 0; j < sqp_intermediate_states.size(); j++) {
sqp_intermediate_states[j].resize(_num_worlds-2);
for (int i = 0; i < _num_worlds-2; i++) {
sqp_intermediate_states[j][i] = new_states.segment((_num_worlds-2)*j + _size_each_state*i, _size_each_state);
}
}
//update controls
vector<VectorXd> new_controls;
new_controls.resize(_num_worlds-1);
for (int i=0; i < _num_worlds-1; i++)
{
new_controls[i] = new_states.segment(_num_traj*_size_each_state*(_num_worlds-2) + i*_size_each_control, _size_each_control);
}
vector<vector<Control*> > c_new_controls;
for (int i = 0; i < current_controls.size(); i++) {
vector<Control*> cu ;
cu.resize(2);
VectorXd vu = current_states[0][i]->JacobianControlWrapper(new_controls[i]);
current_states[0][i]->VectorXdToControl(vu, cu);
for (int j = 0; j < 2; j++) {
cu[j]->setButton(UP, current_controls[i][j]->getButton(UP));
delete current_controls[i][j];
}
current_controls[i].clear();
c_new_controls.push_back(cu);
}
current_controls = c_new_controls;
#if COLOCATION_METHOD
//take sqp takes, transform to real states, and iterate
/*vector< vector<World*> > newStates;
newStates.resize(current_states.size());
for (int j = 0; j < newStates.size(); j++) {
newStates[j].resize(current_states[j].size());
for (int i = 0; i < current_states[j].size(); i++) {
newStates[j][i] = new World(*current_states[j][i]);
delete current_states[j][i]; // TODO: Dependent on memory leak above
current_states[j][i] = NULL;
}
current_states[j].resize(0);
}
current_states.resize(0);*/
//TODO: MOVE THIS OUT OF COLOCATION METHOD!!!
for (int j = 0; j < current_states.size(); j++) {
vector<World*> openLoopWorlds;
openLoopController(current_states[j], current_controls, openLoopWorlds);
openLoopWorlds.pop_back();
openLoopWorlds.push_back(new World(*current_states[j].back()));
for (int i = 0; i < current_states[j].size(); i++) {
delete current_states[j][i];
}
current_states[j] = openLoopWorlds;
}
/*cout << "Projecting SQP States into Legal States" << endl;
boost::thread_group group;
for (int j = 0; j < sqp_intermediate_states.size(); j++) {
for (int i = 0; i < sqp_intermediate_states[j].size(); i++) {
group.create_thread(boost::bind(setWorldFromState,
newStates[j][i+1], &sqp_intermediate_states[j][i]));
}
}
group.join_all();
current_states = newStates;
*/
for (int j = 0; j < current_jacobians.size(); j++) {
for (int i = 0; i < current_jacobians[j].size(); i++) {
current_jacobians[j][i] = MatrixXd();
}
}
#else
assert(false); // not implemented for SHOOTING_METHOD
#endif
#if TIMER_ENABLED
cout << "Jacobian Computation Time: " << JCT << endl;
cout << "MATLAB Solver Time: " << MST << endl;
cout << "Total Solver Time: " << JCT + MST + GetClock() << endl;
#endif
}
int Game_Main(void *parms)
{
// this is the workhorse of your game it will be called
// continuously in real-time this is like main() in C
// all the calls for you game go here!
// game logic here...
// 计时
StartClock();
// 清屏
renderer.fillSurface(0);
/*****************************
处理输入
*****************************/
static DWORD m_deltaTime;
DWORD curTime = timeGetTime();
static DWORD lastTime = curTime;
m_deltaTime = curTime - lastTime;
lastTime = curTime;
if (KEY_DOWN('W') || KEY_DOWN(VK_UP))
{
Vector4 delta = Vector4(0, 0, 0.01f * m_deltaTime, 1);
Matrix4 mrot = camera.MatrixCamera.inverse();
delta = delta * mrot;
camera.position = delta;
}
if (KEY_DOWN('S') || KEY_DOWN(VK_DOWN))
{
Vector4 delta = Vector4(0, 0, -0.01f * m_deltaTime, 1);
Matrix4 mrot = camera.MatrixCamera.inverse();
delta = delta * mrot;
camera.position = delta;
}
if (KEY_DOWN('A') || KEY_DOWN(VK_LEFT))
{
Vector4 delta = Vector4(-0.01f * m_deltaTime, 0, 0, 1);
Matrix4 mrot = camera.MatrixCamera.inverse();
delta = delta * mrot;
camera.position = delta;
}
if (KEY_DOWN('D') || KEY_DOWN(VK_RIGHT))
{
Vector4 delta = Vector4(0.01f * m_deltaTime, 0, 0, 1);
Matrix4 mrot = camera.MatrixCamera.inverse();
delta = delta * mrot;
camera.position = delta;
}
if (KEY_DOWN(VK_PRIOR))
{
Vector4 delta = Vector4(0, -0.01f * m_deltaTime, 0, 1);
Matrix4 mrot = camera.MatrixCamera.inverse();
delta = delta * mrot;
camera.position = delta;
}
if (KEY_DOWN(VK_NEXT))
{
Vector4 delta = Vector4(0, 0.01f * m_deltaTime, 0, 1);
Matrix4 mrot = camera.MatrixCamera.inverse();
delta = delta * mrot;
camera.position = delta;
}
static float ang;
camera.CameraUpdateMatrix();
rend_list.ResetList();
// 表面加锁
renderer.lockSurface();
renderer.clearZBuffer();
//开始
if (rot_pause == 0){
++ang;
}
if (ang >= 360)
ang = 0;
//foreach object
{
//g_Obj.ObjectWorldTransform();
// 每次旋转一下物体
Matrix4 mrot = Matrix4(Quaternion::Euler(0.0f, ang * kPi / 180, 0.0f * kPi / 180));//20.0f * kPi / 180
g_Obj.ObjectTransform(mrot, RENDER_TRANSFORM_LOCALTOTRANS, 0);
g_Obj.ObjectWorldTransform();
}
{
//g_Obj.ObjectWorldTransform();
// 每次旋转一下物体
Matrix4 mrot = Matrix4(Quaternion::Euler(ang * kPi / 180, 0.0f, 0.0f));//20.0f * kPi / 180
g_Obj_c.ObjectTransform(mrot, RENDER_TRANSFORM_LOCALTOTRANS, 0);
g_Obj_c.ObjectWorldTransform();
}
{
Matrix4 mrot = Matrix4(Quaternion::Euler(0.0f, 0.0f, ang * kPi / 180));//20.0f * kPi / 180
g_Obj_s.ObjectTransform(mrot, RENDER_TRANSFORM_LOCALTOTRANS, 0);
g_Obj_s.ObjectWorldTransform();
}
rend_list.InsertGameObject(g_Obj);
rend_list.InsertGameObject(g_Obj_c);
rend_list.InsertGameObject(g_Obj_s);
// 背面消除
rend_list.DeleteBackface(camera);
// 相机变换
rend_list.CameraTransform(camera);
//剪裁
rend_list.ClipPoly(camera);
// 光照
if (open_light == 1) {
directional_light.LightTransform(camera.MatrixCamera);
rend_list.ProcessLight(camera, ambient_light, directional_light);
}
// 投影变换
rend_list.ProjectTransform(camera);
// 视口变换
rend_list.ScreenTransform(camera);
draw_polys = renderer.drawRenderList(rend_list);
// 表面解锁
renderer.unlockSurface();
// 输出
renderer.flipSurface();
// 锁帧
WaitClock();
// return success
return(1);
} // end Game_Main
/* tests thrown in. */
int main( void)
{
struct serial_param sp;
struct interrupt_param irq;
(void)SetNativeSpeed( 14745uL);
/* Desired serial line characteristics 9600,n82 */
sp.baud = 38400uL;
sp.length = UART_WORD_LEN_8;
sp.parity = UART_PARITY_NONE;
sp.stop = UART_STOP_BITS_1;
irq.FIQ = 0;
irq.pri = (INT_PRIORITY)5;
PINSEL0 &= 0x0FFFFu; /* Set pin P0.8-P0.15 to I/O. */
IODIR |= 0xFFFFFF00u; /* Set pin P0.8-P0.32 to output. */
/* Quick toggles to show we got started. */
IOCLR = YELLOW_LED;
IOSET = YELLOW_LED; /* 1 */
IOCLR = YELLOW_LED;
IOSET = YELLOW_LED; /* 2 */
IOCLR = YELLOW_LED;
IOSET = YELLOW_LED; /* 3 */
IOCLR = YELLOW_LED;
IOSET = YELLOW_LED; /* 4 */
IOCLR = YELLOW_LED;
IOSET = YELLOW_LED; /* 5 */
IOCLR = YELLOW_LED;
IOSET = YELLOW_LED; /* 6 */
IOCLR = YELLOW_LED;
/* Set up memory access, CPU and bus speeds. */
(void)SetMAM( 3u, MAM_full_enable);
(void)VPBControl( VPB_DIV1);
(void)SetDesiredSpeed( 60000uL);
(void)ioctl( fileno(stdout), INTERRUPT_SETUP, &irq);
/* Set up serial port to desired rate. */
(void)ioctl( fileno(stdout), UART_SETUP, &sp);
/* Start timer. */
(void)StartClock();
IOSET = YELLOW_LED;
(void)iprintf( "Minimum Wait %u\r\n", MinimumAchievableWait());
IOCLR = YELLOW_LED;
puts( "flasher.c Hello World\r\n"); /* It's alive !! */
printf( "Test done with printf %d\n", 1234 );
IOSET = YELLOW_LED;
/* Another sequence of toggles. This time high and low should */
/* be 1 second each for a 20sec total delay. Should be easily */
/* measureable to confirm timer operation. */
puts( "Starting wait\r\n");
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
WaitUs( 1000000u);
IOCLR = GREEN_LED;
WaitUs( 1000000u);
IOSET = GREEN_LED;
puts( "End wait\r\n");
/* Two possible ways to end, either sit toggling a pin or run */
/* a simple character echo. */
#if 0
PINSEL0 = 0u;
IODIR |= 0x100;
while( 1) { /*lint !e716*/
IOSET = GREEN_LED;
IOCLR = GREEN_LED;
}
#endif
#if 1
while ( 1 )
{
int charAvail;
(void)ioctl( fileno(stdout), UART_CHAR_WAITING, &charAvail );
if ( charAvail )
{
fputc( getchar(), stdout);
}
}
#endif
return( 0); /*lint !e527 unreachable */
}
size_t WeightedMatching(RowStarts rowptr,ColIndices colind,ValueTypePtr C,
ValueTypePtr dist,ValueTypePtr u,ValueTypePtr v,Indices *p,
Indices *m_inv,Indices *m, Indices n,CompareFunction cmpFunc){
typedef typename std::iterator_traits<ValueTypePtr>::value_type ValueType;
Indices i,j,i1,jend,k,m_inv_prev;
Indices match_size=0; Indices k0,j0;
ValueType curr_shortest_path = (ValueType)0;
ValueType curr_aug_path = GetMaxTypeValue<ValueType>(curr_aug_path);
ValueType dist1; Indices itrace;
/*Cost of the edges in the match if
*$(i,j) \in M$ then $clabel[i] = C[i][j]$*/
Indices *clabel = new Indices[n];
Indices *aug_label = new Indices[n];
Indices *update_stack = new Indices[n];
Indices update_stack_index;
/*Save The Operations on the Heap.*/
Indices save_heap_index;
Indices *save_heap_op = new Indices[n];
#ifdef TURN_ON_SAVE_HEAP
double close_factor = (double)1.0 + (double)1.0e-16;
#endif
/*Force the write back to memory to avoid floating point issues*/
ValueType force_mem_write[3];
#ifndef NO_LOCAL_PROFILING
CreateProfilingClocks();
#endif
/*Core Profiling Clock*/
Clock *core_clk = CreateClock();
#ifdef USE_BIT_ARRAY
BitArray_t *col_marker = CreateBitArray(n);
BitArray_t *heap_marker = CreateBitArray(n);
#else
Indices *col_marker = new Indices[n];
unsigned int *heap_marker = NULL;
col_marker--;
for(i=1;i<=n;i++){ /*Do we need Initialization?*/
col_marker[i] = (Indices)0;
}
#endif
#if BINARY_HEAP
Heap *bin_heap = NewBinaryHeap(cmpFunc,n,GetDistKeyID);
ValueType *dist_ptr = NULL;
heap_marker = bin_heap->keyMap;
#endif
/*Algorithm Uses 1-Indexing to be consistent*/
C--;m--;dist--;u--;v--;p--;m_inv--;
rowptr--;colind--;clabel--;save_heap_op--;
update_stack--;aug_label--;
assert(dist && u && v && p);
ComputeInitialExtremeMatch<ValueType,Indices>(u,v,clabel,C,m,m_inv,colind,
rowptr,n,dist);
match_size=0;
StartClock(core_clk);
for(i=1;i<=n;i++){
if(m_inv[i]){
match_size++;
continue;
}
/*
*Aim is to find a value for jend such that the path
*from i-->jend is the shortest
*/
i1 = i; p[i1] = 0; jend=0; itrace=i;
#ifdef USE_BIT_ARRAY
ResetAllBits(col_marker);
ResetAllBits(heap_marker);
#endif
#if BINARY_HEAP
bin_heap->n = 0;
dist_base = (unsigned long)&(dist[1]);
#endif
curr_shortest_path=(ValueType)0;
curr_aug_path=GetMaxTypeValue<ValueType>(curr_aug_path);
save_heap_index = (Indices)0;
update_stack_index = (Indices)0;
while(1){
for(k=rowptr[i1]+1;k<(rowptr[i1+1]+1);k++){
j = colind[k]+1;
#ifdef USE_BIT_ARRAY
if(CheckBit(col_marker,j)){
#else
if(col_marker[j]==i){
#endif
continue;
}
force_mem_write[k%3] = C[k]-(v[i1]+u[j]);
dist1 = curr_shortest_path + force_mem_write[k%3];
/*Prune any dist1's > curr_aug_path, since
*all the costs>0
*/
if(dist1 < curr_aug_path){
if(!m[j]){
/*we need itrace because, the last i1 which
*we explore may not actually give the shortest
*augmenting path.*/
jend = j; itrace = i1;
curr_aug_path = dist1;
aug_label[j] = k;
}else if(dist1 < dist[j]){
/*Update the dist*/
dist[j] = dist1; p[m[j]] = i1;
aug_label[j] = k;
#if SIMPLE_HEAP
#ifdef USE_BIT_ARRAY
SetBit(heap_marker,j);
#else
heap_marker[j] = i;
#endif
#elif BINARY_HEAP /*SIMPLE_HEAP*/
#ifdef USE_BIT_ARRAY
if(CheckBit(heap_marker,j)){
#else
if(heap_marker[j]){
#endif
#ifndef NO_LOCAL_PROFILING
StartClock(hupdate_clk);
#endif
/*Call the decrease Key Operation*/
DecreaseKey(bin_heap,j);
#ifndef NO_LOCAL_PROFILING
StopClock(hupdate_clk);
hupdate_ticks += GetClockTicks(hupdate_clk);
#endif
}
#ifdef TURN_ON_SAVE_HEAP
else if(curr_shortest_path &&
dist[j] <= (curr_shortest_path)*(close_factor)){
/*If dist[j] is close to root push it in
*save_heap_op*/
assert(save_heap_index < n);
save_heap_op[++save_heap_index] = j;
}
#endif
else{
#ifndef NO_LOCAL_PROFILING
StartClock(hins_clk);
#endif
InsertHeap(bin_heap,&(dist[j]));
#ifndef NO_LOCAL_PROFILING
StopClock(hins_clk);
hins_ticks += GetClockTicks(hins_clk);
#endif
#ifdef USE_BIT_ARRAY
SetBit(heap_marker,j);
#endif
}
#endif /*SIMPLE_HEAP*/
}
}
}
if(curr_aug_path <= curr_shortest_path){
break;
}
/*We now have a heap of matched cols, so pick the min*/
#ifdef SIMPLE_HEAP
j = SimplePickMin(heap_marker,dist,n);
if(j){
curr_shortest_path = dist[j];
UnsetBit(heap_marker,j);
#elif BINARY_HEAP
#ifndef NO_LOCAL_PROFILING
StartClock(hdel_clk);
#endif
if(save_heap_index){
j = save_heap_op[save_heap_index];
save_heap_index--;
curr_shortest_path = dist[j];
#ifdef USE_BIT_ARRAY
SetBit(col_marker,j);
#else
col_marker[j] = (Indices)i;
update_stack[++update_stack_index]=j;
#endif /*#ifdef USE_BIT_ARRAY*/
i1 = m[j];
}else if(dist_ptr = (ValueType *) HeapDelete(bin_heap)) {
#ifndef NO_LOCAL_PROFILING
StopClock(hdel_clk);
hdel_ticks += GetClockTicks(hdel_clk);
#endif
assert((unsigned long)dist_ptr >= (unsigned long)&dist[1]);
j = ((((unsigned long)dist_ptr -
(unsigned long)&dist[1]))/sizeof(ValueType))+1;
assert(j>=1 && j<=n);
curr_shortest_path = dist[j];
heap_marker[j] = 0; /*Setting the keyMap in Heap to 0*/
#endif /*#ifdef SIMPLE_HEAP */
#ifdef USE_BIT_ARRAY
SetBit(col_marker,j);
update_stack[++update_stack_index]=j;
#else
col_marker[j] = (Indices)i;
update_stack[++update_stack_index]=j;
#endif /*#ifdef USE_BIT_ARRAY*/
i1 = m[j];
}else{
break;
}
}
/*We found a shortest augmenting path*/
if(jend){
unsigned long **harray = bin_heap->heapArray;
#ifndef NO_LOCAL_PROFILING
StartClock(dual_clk);
#endif
/*NOTE1: We need a very fast way to update
*the dual variables and also reset the dist[]
*we avoid linear scan where ever we can to update
*these dual variables*/
while(update_stack_index){ /*Update u[j]: while*/
j=update_stack[update_stack_index--];
u[j] = (u[j]+dist[j])-curr_aug_path;
if(m[j]){ /*See NOTE1*/
i1 = m[j];
v[i1] = C[clabel[i1]] - u[j];
}
dist[j] = MAX_DOUBLE_VALUE;
if(bin_heap->n){
dist_ptr = (double *)harray[bin_heap->n];
j = ((((unsigned long)dist_ptr -
(unsigned long)&dist[1]))/sizeof(ValueType))+1;
heap_marker[j] = 0;
*((double *)harray[bin_heap->n]) = MAX_DOUBLE_VALUE;
bin_heap->n -= 1 ;
}
} /*Update u[j]: while*/
/*Uncomment if you need to print augmenting path*/
/*node_t itrace_prev;*/
/*printf("Shortest augmenting Path {");*/
j=jend;
while(itrace){
m_inv_prev = m_inv[itrace];
m[j] = itrace; m_inv[itrace]=j;
/*See NOTE1(above)*/
clabel[itrace] = aug_label[j];
v[itrace] = C[clabel[itrace]] - u[j];
/*printf("(%u,%u)",itrace,j);*/
j=m_inv_prev;
/*itrace_prev = itrace;*/
itrace = p[itrace];
/*
if(itrace){
printf("(%u,%u)",itrace_prev,m_inv_prev);
}*/
}
/*printf("}\n");*/
/*There may some dist[] still in the heap*/
while(bin_heap->n){
dist_ptr = (double *)harray[bin_heap->n];
j = ((((unsigned long)dist_ptr -
(unsigned long)&dist[1]))/sizeof(ValueType))+1;
heap_marker[j] = 0;
*((double *)harray[bin_heap->n]) = MAX_DOUBLE_VALUE;
bin_heap->n -= 1;
}
match_size++;
/*End Dual Update*/
#ifndef NO_LOCAL_PROFILING
StopClock(dual_clk);
dual_ticks += GetClockTicks(dual_clk);
#endif
} /*if(jend) : Found Augmeting Path*/
} /*for(i=1;i<=n;i++): Main Outer Loop*/
StopClock(core_clk);
WeightedMatchTicks = GetClockTicks(core_clk);
#ifndef NO_LOCAL_PROFILING
printf("Profile Summary\n");
printf("HINS=(%d) HDEL=(%d) HUPDATE=(%d)\n",(int)hins_ticks,(int)hdel_ticks,
(int)hupdate_ticks);
printf("DUAL=(%d) \n",(int)dual_ticks);
#endif
#ifdef USE_BIT_ARRAY
FreeBitArray(col_marker);
FreeBitArray(heap_marker);
#else
col_marker++;
delete col_marker;
#endif
#ifdef SIMPLE_HEAP
heap_marker++;
delete heap_marker;
#endif
aug_label++;
delete aug_label;
save_heap_op++;
delete save_heap_op;
clabel++;
delete clabel;
#ifdef BINARY_HEAP
FreeHeap(bin_heap);
#endif
return match_size;
}
/*O(n) time picking the maximum from the heap_marker */
node_t SimplePickMin(BitArray_t *bit_heap,double *dist,node_t n){
node_t min_j=0;node_t j;
double curr_min = HUGE_VAL;
for(j=1;j<=n;j++){
if(CheckBit(bit_heap,j) && dist[j] < curr_min){
min_j = j;
curr_min = dist[j];
}
}
return min_j;
}
#ifdef BINARY_HEAP
inline keyid_t GetDistKeyID(void *dist_ptr){ assert((unsigned long)dist_ptr >= dist_base);
return (((((unsigned long)dist_ptr-dist_base))/sizeof(double))+1);
}
#endif
BitArray_t* CreateBitArray(unsigned int size){
div_t d = div(size,SIZE_OF_BYTE_IN_BITS);
BitArray_t *bits = (BitArray_t *)malloc(sizeof(BitArray_t)*1);
assert(bits);
bits->size = (d.rem > 0)?(d.quot+1):d.quot;
bits->ba = (char *)malloc(sizeof(char)*(bits->size));
assert(bits->ba);
memset(bits->ba,'\0',bits->size);
return bits;
}
void ComputeInitialExtremeMatch(ValueType *u,ValueType *v,Indices *clabel,ValueType *C,
Indices *m,Indices *m_inv,Indices* colind,Indices* rowptr,Indices n,ValueType *dist){
Indices i,k,i1,k1;
Indices k0,k10,j0,j,j1,j10;
ValueType vmin;
ValueType C1k;
Clock *init_match_clk = CreateClock();
StartClock(init_match_clk);
#if 0
/*Compute u[j]*/
for(j=1;j<=n;j++){
u[j] = GetMaxTypeValue<ValueType>(u[j]);
dist[j] = u[j];
m[j]=0;m_inv[j]=0;
}
for(i=1;i<=n;i++){
for(k=rowptr[i];k<rowptr[i+1];k++){
if(C[k]<u[colind[k]]){
u[colind[k]] = C[k];
}
}
}
#endif
/*Compute v[i]*/
for(i=1;i<=n;i++){
v[i] = GetMaxTypeValue<ValueType>(v[i]);
for(k=rowptr[i]+1;k<(rowptr[i+1]+1);k++){
j = colind[k]+1;
vmin = (C[k]-u[j]);
if(vmin < v[i]){
v[i] = vmin;
}
}
}
/*Update Cost and match.*/
for(i=1;i<=n;i++){
for(k=rowptr[i]+1;k<(rowptr[i+1]+1);k++){
j = colind[k]+1;
C1k = C[k]-v[i]-u[j]; /*to handle -0.0*/
if(fabs(C1k-0.0) <= 1.0e-30 && (!m[j] && !m_inv[i])){
m[j] = i;
m_inv[i] = j;
clabel[i] = k;
}
}
}
/*1-Step Augmentation*/
for(i=1;i<=n;i++){
if(!m_inv[i]){ /*Unmatched Row*/
for(k=rowptr[i]+1;k<(rowptr[i+1]+1) && !(m_inv[i]);k++){
j = colind[k]+1;
C1k = fabs(C[k]-v[i]-u[j]);
if(C1k <= 1.0e-30){
/*assert(m[colind[k]]);*/
i1 = m[j];
/*assert(m_inv[i1] == j);*/
/*See if we can find any C1(i1,j1) == 0*/
for(k1=rowptr[i1]+1;k1<(rowptr[i1+1]+1);k1++){
j1 = colind[k1]+1;
C1k = fabs(C[k1] - v[i1]-u[j1]);
if(C1k <= 1.0e-30 && !(m[j1])){
/*augment the match.*/
m[j] = i;
m_inv[i] = j;
clabel[i] = k;
m[j1] = i1;
m_inv[i1] = j1;
clabel[i1] = k1;
break;
}
}
}
}
}
}
StopClock(init_match_clk);
InitialMatchTicks = GetClockTicks(init_match_clk);
}
