bool VDThread::ThreadStart() {
VDASSERT(!isThreadAttached());
if (!isThreadAttached())
mhThread = (void *)_beginthreadex(NULL, 0, StaticThreadStart, this, 0, &mThreadID);
return mhThread != 0;
}
bool VDThread::ThreadStart() {
VDASSERT(!isThreadAttached());
if (!isThreadAttached()) {
mhThread = (void *)_beginthreadex(NULL, 0, StaticThreadStart, this, 0, &mThreadID);
if (mhThread && mThreadPriority != INT_MIN)
::SetThreadPriority(mhThread, mThreadPriority);
}
return mhThread != 0;
}
void Needle::setTransformFromEndEffectorBoxConstrained(const Vector3d& new_ee_pos, const Matrix3d& new_ee_rot, Box* box)
{
Vector3d old_ee_pos;
Matrix3d old_ee_rot;
getEndEffectorTransform(old_ee_pos, old_ee_rot);
Vector3d old_proj = (old_ee_pos - position);
old_proj.normalize();
Vector3d proj = (new_ee_pos-position).dot(rotation.col(0))*rotation.col(0) + (new_ee_pos-position).dot(rotation.col(2))*rotation.col(2);
proj.normalize();
glColor3f(0.8,0.8,0);
drawArrow(position, 20*proj);
glColor3f(0.8,0,0);
drawArrow(position, 20*old_proj);
Vector3d normal = proj.cross(old_proj);
normal.normalize();
double angle = angle_between(proj, old_proj)*180.0/M_PI;
if (angle > 90.0) {
angle = 0.0;
//TODO return;
} else {
double sign = ((normal - rotation.col(1)).squaredNorm() < 1e-5) ? -1 : 1;
angle = sign*min(angle_between(proj, old_proj)*180.0/M_PI, 5.0);
}
if (!isnan(angle) && abs(angle)>1e-5 && angle>0.0) { //TODO for now, only allows forward movement of needle through box
rotateAboutAxis(angle);
if(isThreadAttached()) {
thread->updateConstrainedTransform(constraint_ind, getEndPosition(), getEndRotation());
}
}
if (isThreadAttached()) {
Vector3d direction;
if (!box->isThreadAttached() && (sphereBoxDistance(getEndPosition(), getThicknessRadius(), box->getPosition(), box->getHalfLength(), direction) < 0)) {
cout << "thread gets into the box" << endl;
//box->attach(getThread());
box->attachThreadIn(getThread(), getEndPosition(), getEndRotation());
}
if (box->isThreadAttached() && !boxCollision(box->getPosition(), box->getHalfLength())) {
cout << "thread gets out of the box" << endl;
box->attachThreadOut(getThread(), getEndPosition(), getEndRotation());
}
}
}
bool VDAudioOutputDirectSoundW32::Finalize(uint32 timeout) {
DWORD deadline = GetTickCount() + timeout;
mMutex.Lock();
for(;;) {
if (mThreadState != kThreadStatePlay || !isThreadAttached() || mDSStreamPlayPosition == mStreamWritePosition)
break;
mMutex.Unlock();
if (timeout == (uint32)-1)
mResponseEvent.wait();
else {
uint32 timeNow = GetTickCount();
sint32 delta = deadline - timeNow;
if (delta < 0)
return false;
mResponseEvent.tryWait(delta);
}
mMutex.Lock();
}
mMutex.Unlock();
return true;
}
void VDThread::ThreadWait() {
if (isThreadAttached()) {
WaitForSingleObject((HANDLE)mhThread, INFINITE);
ThreadDetach();
mThreadID = 0;
}
}
void VDThread::ThreadDetach() {
if (isThreadAttached()) {
CloseHandle((HANDLE)mhThread);
mhThread = NULL;
mThreadID = 0;
}
}
void Needle::rotateAboutAxis(double degrees)
{
setTransform(position, AngleAxisd(degrees * M_PI/180.0, rotation.col(1)) * rotation);
if(isThreadAttached()) {
thread->updateConstrainedTransform(constraint_ind, getEndPosition(), getEndRotation());
}
}
bool VDThread::isThreadActive() {
if (isThreadAttached()) {
if (WAIT_TIMEOUT == WaitForSingleObject((HANDLE)mhThread, 0))
return true;
ThreadDetach();
mThreadID = 0;
}
return false;
}
void Needle::updateTransformFromAttachment()
{
if (isThreadAttached()) {
const Matrix3d thread_rot = thread->rotationAtConstraint(constraint_ind);
const Vector3d thread_pos = thread->positionAtConstraint(constraint_ind);
const Matrix3d needle_rot = AngleAxisd(angle * M_PI/180.0, thread_rot.col(1)) * thread_rot;
const Vector3d needle_pos = thread_pos - radius * (AngleAxisd(-angle * M_PI/180.0, needle_rot.col(1)) * needle_rot).col(2);
setTransform(needle_pos, needle_rot);
}
}
void Needle::setTransformFromEndEffector(const Vector3d& ee_pos, const Matrix3d& ee_rot)
{
vector<Box*> boxes;
world->getObjects<Box>(boxes);
Box* box = NULL;
for (int i = 0; i < boxes.size(); i++) {
if (boxes[i]->isNeedleAttached() && (boxes[i]->getNeedle() == this)) {
box = boxes[i];
break;
}
}
if (box == NULL) {
setTransform(ee_pos + ee_rot*rotation_offset*position_offset, ee_rot * rotation_offset);
if(isThreadAttached()) {
vector<int> constraint_inds;
thread->getConstrainedVerticesNums(constraint_inds);
if (constraint_inds.size() != 3) {
cout << "adding constraint" << endl;
thread->addConstraint(10);
//thread->updateRotationOffset(1, (Matrix3d) Angle;
}
Vector3d position_diff = thread->rotationAtConstraint(1).transpose() * (thread->positionAtConstraint(1) - thread->positionAtConstraint(0));
position_diff = (thread->positionAtConstraint(1) - thread->positionAtConstraint(0));
//Matrix3d rotation_diff = thread->rotationAtConstraint(0).transpose() * thread->rotationAtConstraint(1);
Matrix3d rotation_diff = thread->rotationAtConstraint(0).transpose() * thread->rotationAtConstraint(1);
assert(constraint_ind == 0);
//thread->updateConstrainedTransform(constraint_ind, getEndPosition(), getEndRotation());
thread->updateConstrainedTransform(0, getEndPosition(), getEndRotation());
Matrix3d ee_rot = thread->rotationAtConstraint(0) * rotation_diff;
//ee_rot = (Matrix3d) AngleAxisd(ee_rot);
// ee_rot.col(1) = ee_rot.col(2).cross(ee_rot.col(0));
// ee_rot.col(2) = ee_rot.col(0).cross(ee_rot.col(1));
// ee_rot.col(0).normalize();
// ee_rot.col(1).normalize();
// ee_rot.col(2).normalize();
//position_diff = Vector3d(-10,0,0);
//thread->updateConstrainedTransform(1, ee_rot*position_diff + getEndPosition(), ee_rot);
thread->updateConstrainedTransform(1, position_diff + getEndPosition(), ee_rot); //thread->rotationAtConstraint(1);
//thread->updateConstrainedRawTransform(1, position_diff + getEndPosition(), ee_rot, ee_rot);
//thread->updateConstrainedRawTransform(1, ee_rot*position_diff + getEndPosition(), ee_rot, ee_rot);
//thread->minimize_energy();
}
} else {
setTransformFromEndEffectorBoxConstrained(ee_pos, ee_rot, box);
}
checkBoxAttach();
}
void VDFileAsyncNT::SafeTruncateAndClose(sint64 pos) {
if (isThreadAttached()) {
mState = kStateAbort;
mWriteOccurred.signal();
ThreadWait();
if (mpError) {
delete mpError;
mpError = NULL;
}
}
if (mhFileSlow != INVALID_HANDLE_VALUE) {
Extend(pos);
Close();
}
}
void *VDThread::ThreadLocation() const {
if (!isThreadAttached())
return NULL;
CONTEXT ctx;
ctx.ContextFlags = CONTEXT_CONTROL;
SuspendThread(mhThread);
GetThreadContext(mhThread, &ctx);
ResumeThread(mhThread);
#ifdef _M_AMD64
return (void *)ctx.Rip;
#else
return (void *)ctx.Eip;
#endif
}
VDThread::~VDThread() throw() {
if (isThreadAttached())
ThreadWait();
}
