void* dNewtonBody::GetInterpolatedRotation()
{
const dNewtonWorld* const world = (dNewtonWorld*) NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));
ScopeLock scopelock(&m_lock);
m_interpolatedRotation = m_rotation0.Slerp(m_rotation1, world->m_interpotationParam);
return &m_interpolatedRotation.m_q0;
}
void rayPickerManager::PreUpdate(dFloat timestep)
{
// all of the work will be done here;
dNewton::ScopeLock scopelock (&m_lock);
if (m_pickedBody) {
if (m_pickedBody->GetType() == dNewtonBody::m_dynamic) {
newtonDynamicBody* const body = (newtonDynamicBody*)m_pickedBody;
dFloat invTimeStep = 1.0f / timestep;
Matrix matrix (body->GetMatrix());
Vec4 omega0 (body->GetOmega());
Vec4 veloc0 (body->GetVeloc());
Vec4 peekPosit (m_localpHandlePoint * matrix);
Vec4 peekStep (m_globalTarget - peekPosit);
Vec4 pointVeloc (body->GetPointVeloc (peekPosit));
Vec4 deltaVeloc (peekStep * (m_stiffness * invTimeStep) - pointVeloc);
for (int i = 0; i < 3; i ++) {
Vec4 veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
body->ApplyImpulseToDesiredPointVeloc (peekPosit, veloc);
}
// damp angular velocity
Vec4 omega1 (body->GetOmega());
Vec4 veloc1 (body->GetVeloc());
omega1 = omega1 * (0.9f);
// restore body velocity and angular velocity
body->SetVeloc(veloc0);
body->SetOmega(omega0);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
body->GetMassAndInertia (mass, Ixx, Iyy, Izz);
matrix.setTrans(Vec3(0.0f, 0.0f, 0.0f));
Vec4 relOmega (omega1 - omega0);
relOmega = matrix.preMult(relOmega);
Vec4 angularMomentum (Ixx, Iyy, Izz, 0.0f);
angularMomentum = componentMultiply (relOmega, angularMomentum);
angularMomentum = matrix.postMult(angularMomentum);
Vec4 torque (angularMomentum * invTimeStep);
body->AddTorque(torque);
Vec4 relVeloc (veloc1 - veloc0);
Vec4 force (relVeloc * (mass * invTimeStep));
body->AddForce (force);
} else {
dAssert (0);
}
}
}
void* dNewtonBody::GetInterpolatedPosition()
{
const dNewtonWorld* const world = (dNewtonWorld*)NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));
ScopeLock scopelock(&m_lock);
m_interpolatedPosit = m_posit0 + (m_posit1 - m_posit0).Scale(world->m_interpotationParam);
return &m_interpolatedPosit.m_x;
}
void dNewtonBody::OnBodyTransform(const dFloat* const matrixPtr, int threadIndex)
{
dMatrix matrix(matrixPtr);
ScopeLock scopelock(&m_lock);
m_posit0 = m_posit1;
m_rotation0 = m_rotation1;
m_posit1 = matrix.m_posit;
m_rotation1 = dQuaternion(matrix);
dFloat angle = m_rotation0.DotProduct(m_rotation1);
if (angle < 0.0f) {
m_rotation1.Scale(-1.0f);
}
}
void rayPickerManager::SetPickedBody (dNewtonBody* const body, const Vec4& handle)
{
dNewton::ScopeLock scopelock (&m_lock);
m_pickedBody = body;
if (m_pickedBody) {
Matrix matrix;
if (m_pickedBody->GetType() == dNewtonBody::m_dynamic) {
matrix.invert(((newtonDynamicBody*) body)->GetMatrix());
} else {
dAssert (0);
}
m_localpHandlePoint = handle * matrix;
m_globalTarget = handle;
}
}
/**
* routine executed by worker threads.
*
* @function worker_routine
*
* @date 2016-01-15
*
* @revision none
*
* @designer Eric Tsang
*
* @programmer Eric Tsang
*
* @note
*
* continuously reads tasks from the tasks vector, executes them, and writes the
* results to the results pipe for the parent to receive.
*
* once there are no more tasks to execute, the thread terminates.
*
* @signature void* worker_routine(void*)
*/
void* worker_routine(void*)
{
bool yield = false;
while(true)
{
if (yield)
{
pthread_yield();
yield = false;
}
Number* loBoundPtr;
// get the next task that needs processing
{
Lock scopelock(&taskAccess.sem);
// if there are tasks available to get, get them
if(!tasks.empty())
{
loBoundPtr = tasks.back();
tasks.pop_back();
}
// otherwise, if there are tasks available in the future, wait for
// them to be available
else if(!allTasksProduced)
{
yield = true;
continue;
}
// otherwise, there are no more tasks, end the loop
else
{
break;
}
}
tasksNotFullSem.post();
// calculate the hiBound for a task
Number hiBound;
mpz_add_ui(hiBound.value,loBoundPtr->value,MAX_NUMBERS_PER_TASK-1);
if(mpz_cmp(hiBound.value,prime.value) > 0)
{
mpz_set(hiBound.value,prime.value);
}
// create the task
FindFactorsTask newTask(prime.value,hiBound.value,loBoundPtr->value);
// do the processing
newTask.execute();
// post results of the tasks
{
Lock scopelock(&resultAccess.sem);
std::vector<mpz_t*>* taskResults = newTask.get_results();
for(register unsigned int i = 0; i < taskResults->size(); ++i)
{
Number* numPtr = new Number();
mpz_set(numPtr->value,*taskResults->at(i));
results.push_back(numPtr);
}
}
delete loBoundPtr;
}
pthread_exit(0);
}
/**
* entry point of the program.
*
* @function main
*
* @date 2016-01-15
*
* @revision none
*
* @designer Eric Tsang
*
* @programmer Eric Tsang
*
* @note
*
* sets up synchronization primitives, spawns worker threads, generates tasks
* for workers, receives results from workers, waits for workers to terminate,
* writes results to a file, and stdout.
*
* @signature int main(int argc,char** argv)
*
* @param argc number of command line arguments
* @param argv array of c strings of command line arguments
*
* @return status code.
*/
int main(int argc,char** argv)
{
// parse command line arguments
if (argc != 4)
{
fprintf(stderr,"usage: %s [integer] [path to log file] [num workers]\n",argv[0]);
return 1;
}
if(mpz_set_str(prime.value,argv[1],10) == -1)
{
fprintf(stderr,"usage: %s [integer] [path to log file] [num workers]\n",argv[0]);
return 1;
}
int logfile = open(argv[2],O_CREAT|O_WRONLY|O_APPEND);
FILE* logFileOut = fdopen(logfile,"w");
if(logfile == -1 || errno)
{
fprintf(stderr,"usage: %s [integer] [path to log file] [num workers]\nerror occurred: ",argv[0]);
perror(0);
return 1;
}
unsigned int numWorkers = atoi(argv[3]);
if (numWorkers <= 0)
{
fprintf(stderr,"usage: %s [integer] [path to log file] [num workers]\n num workers must be larger than or equal to 1",argv[0]);
return 1;
}
// set up synchronization primitives
for(register unsigned int i; i < numWorkers*MAX_PENDING_TASKS_PER_WORKER; ++i)
{
tasksNotFullSem.post();
}
// get start time
long startTime = current_timestamp();
// create the worker threads
std::vector<pthread_t> workers;
for(register unsigned int i = 0; i < numWorkers; ++i)
{
pthread_t worker;
pthread_create(&worker,0,worker_routine,0);
workers.push_back(worker);
}
// create tasks and place them into the tasks vector
{
Number prevPercentageComplete;
Number percentageComplete;
Number tempLoBound;
Number loBound;
for(mpz_set_ui(loBound.value,1);
mpz_cmp(loBound.value,prime.value) <= 0;
mpz_add_ui(loBound.value,loBound.value,MAX_NUMBERS_PER_TASK))
{
// calculate and print percentage complete
mpz_set(prevPercentageComplete.value,percentageComplete.value);
mpz_mul_ui(tempLoBound.value,loBound.value,100);
mpz_div(percentageComplete.value,tempLoBound.value,prime.value);
if(mpz_cmp(prevPercentageComplete.value,percentageComplete.value) != 0)
{
gmp_fprintf(stdout,"%Zd%\n",percentageComplete.value);
gmp_fprintf(logFileOut,"%Zd%\n",percentageComplete.value);
}
// insert the task into the task queue once there is room
Number* newNum = new Number();
mpz_set(newNum->value,loBound.value);
tasksNotFullSem.wait();
Lock scopelock(&taskAccess.sem);
tasks.push_back(newNum);
}
}
allTasksProduced = true;
// join all worker threads
for(register unsigned int i = 0; i < numWorkers; ++i)
{
void* unused;
pthread_join(workers[i],&unused);
}
// get end time
long endTime = current_timestamp();
// print out calculation results
std::sort(results.begin(),results.end(),[](Number* i,Number* j)
{
return mpz_cmp(i->value,j->value) < 0;
});
fprintf(stdout,"factors: ");
fprintf(logFileOut,"factors: ");
for(register unsigned int i = 0; i < results.size(); ++i)
{
gmp_fprintf(stdout,"%s%Zd",i?", ":"",results[i]->value);
gmp_fprintf(logFileOut,"%s%Zd",i?", ":"",results[i]->value);
delete results[i];
}
fprintf(stdout,"\n");
fprintf(logFileOut,"\n");
// print out execution results
fprintf(stdout,"total runtime: %lums\n",endTime-startTime);
fprintf(logFileOut,"total runtime: %lums\n",endTime-startTime);
// release system resources
fclose(logFileOut);
close(logfile);
return 0;
}
void rayPickerManager::SetTarget (const Vec4& targetPoint)
{
dNewton::ScopeLock scopelock (&m_lock);
m_globalTarget = targetPoint;
}