dgBigVector dgCollisionConvexHull::FaceNormal (const dgEdge *face, const dgBigVector* const pool) const
{
const dgEdge* edge = face;
dgBigVector p0 (pool[edge->m_incidentVertex]);
edge = edge->m_next;
dgBigVector p1 (pool[edge->m_incidentVertex]);
dgBigVector e1 (p1 - p0);
dgBigVector normal (dgFloat32 (0.0f));
for (edge = edge->m_next; edge != face; edge = edge->m_next) {
dgBigVector p2 (pool[edge->m_incidentVertex]);
dgBigVector e2 (p2 - p0);
dgBigVector n1 (e1.CrossProduct(e2));
#ifdef _DEBUG
dgAssert(n1.m_w == dgFloat32(0.0f));
dgFloat64 mag = normal.DotProduct(n1).GetScalar();
dgAssert ( mag >= -dgFloat32 (0.1f));
#endif
normal += n1;
e1 = e2;
}
dgFloat64 den = sqrt (normal.DotProduct(normal).GetScalar()) + dgFloat64 (1.0e-24f);
normal = normal.Scale (dgFloat64 (1.0f)/ den);
#ifdef _DEBUG
edge = face;
dgBigVector e0 (pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]);
do {
dgBigVector de1 (pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]);
dgBigVector dn1 (e0.CrossProduct(de1));
dgFloat64 x = normal.DotProduct(dn1).GetScalar();
dgAssert (x > -dgFloat64 (0.01f));
e0 = de1;
edge = edge->m_next;
} while (edge != face);
#endif
return normal;
}
/*public*/
void
PlanarGraph::addEdges(const vector<Edge*>& edgesToAdd)
{
// create all the nodes for the edges
for(vector<Edge*>::const_iterator it = edgesToAdd.begin(),
endIt = edgesToAdd.end(); it != endIt; ++it) {
Edge* e = *it;
assert(e);
edges->push_back(e);
// PlanarGraph destructor will delete all DirectedEdges
// in edgeEndList, which is where these are added
// by the ::add(EdgeEnd) call
std::unique_ptr<DirectedEdge> de1(new DirectedEdge(e, true));
std::unique_ptr<DirectedEdge> de2(new DirectedEdge(e, false));
de1->setSym(de2.get());
de2->setSym(de1.get());
// First, ::add takes the ownership, then follows with operations that may throw.
add(de1.release());
add(de2.release());
}
}
static void
executeOneCommand(JNIEnv* env) {
switch (saCmdType) {
case SA_CMD_SUSPEND_ALL: {
if (suspended) {
reportErrorToSA("Target process already suspended");
return;
}
// We implement this by getting all of the threads and calling
// SuspendThread on each one, except for the thread object
// corresponding to this thread. Each thread for which the call
// succeeded (i.e., did not return JVMDI_ERROR_INVALID_THREAD)
// is added to a list which is remembered for later resumption.
// Note that this currently has race conditions since a thread
// might be started after we call GetAllThreads and since a
// thread for which we got an error earlier might be resumed by
// the VM while we are busy suspending other threads. We could
// solve this by looping until there are no more threads we can
// suspend, but a more robust and scalable solution is to add
// this functionality to the JVMDI interface (i.e.,
// "suspendAll"). Probably need to provide an exclude list for
// such a routine.
jint threadCount;
jthread* threads;
if (jvmdi->GetAllThreads(&threadCount, &threads) != JVMDI_ERROR_NONE) {
reportErrorToSA("Error while getting thread list");
return;
}
for (int i = 0; i < threadCount; i++) {
jthread thr = threads[i];
if (!env->IsSameObject(thr, debugThreadObj)) {
jvmdiError err = jvmdi->SuspendThread(thr);
if (err == JVMDI_ERROR_NONE) {
// Remember this thread and do not free it
suspendedThreads.push_back(thr);
continue;
} else {
fprintf(stderr, " SA: Error %d while suspending thread\n", err);
// FIXME: stop, resume all threads, report error
}
}
env->DeleteGlobalRef(thr);
}
// Free up threads
jvmdi->Deallocate((jbyte*) threads);
// Suspension is complete
suspended = true;
break;
}
case SA_CMD_RESUME_ALL: {
if (!suspended) {
reportErrorToSA("Target process already suspended");
return;
}
saCmdResult = 0;
bool errorOccurred = false;
jvmdiError firstError;
for (int i = 0; i < suspendedThreads.size(); i++) {
jthread thr = suspendedThreads[i];
jvmdiError err = jvmdi->ResumeThread(thr);
env->DeleteGlobalRef(thr);
if (err != JVMDI_ERROR_NONE) {
if (!errorOccurred) {
errorOccurred = true;
firstError = err;
}
}
}
suspendedThreads.clear();
suspended = false;
if (errorOccurred) {
reportErrorToSA("Error %d while resuming threads", firstError);
return;
}
break;
}
case SA_CMD_TOGGLE_BREAKPOINT: {
saCmdBkptResWasError = 1;
// Search line number info for all loaded classes
jint classCount;
jclass* classes;
jvmdiError glcRes = jvmdi->GetLoadedClasses(&classCount, &classes);
if (glcRes != JVMDI_ERROR_NONE) {
reportErrorToSA("Error %d while getting loaded classes", glcRes);
return;
}
JvmdiRefListDeallocator rld(env, (jobject*) classes, classCount);
bool done = false;
bool gotOne = false;
jclass targetClass;
jmethodID targetMethod;
jlocation targetLocation;
jint targetLineNumber;
for (int i = 0; i < classCount && !done; i++) {
fflush(stderr);
jclass clazz = classes[i];
char* srcName;
jvmdiError sfnRes = jvmdi->GetSourceFileName(clazz, &srcName);
if (sfnRes == JVMDI_ERROR_NONE) {
JvmdiDeallocator de1(srcName);
if (!strcmp(srcName, saCmdBkptSrcFileName)) {
// Got a match. Now see whether the package name of the class also matches
char* clazzName;
jvmdiError sigRes = jvmdi->GetClassSignature(clazz, &clazzName);
if (sigRes != JVMDI_ERROR_NONE) {
reportErrorToSA("Error %d while getting a class's signature", sigRes);
return;
}
JvmdiDeallocator de2(clazzName);
if (packageNameMatches(clazzName + 1, saCmdBkptPkgName)) {
// Iterate through all methods
jint methodCount;
jmethodID* methods;
if (jvmdi->GetClassMethods(clazz, &methodCount, &methods) != JVMDI_ERROR_NONE) {
reportErrorToSA("Error while getting methods of class %s", clazzName);
return;
}
JvmdiDeallocator de3(methods);
for (int j = 0; j < methodCount && !done; j++) {
jmethodID m = methods[j];
jint entryCount;
JVMDI_line_number_entry* table;
jvmdiError lnRes = jvmdi->GetLineNumberTable(clazz, m, &entryCount, &table);
if (lnRes == JVMDI_ERROR_NONE) {
JvmdiDeallocator de4(table);
// Look for line number greater than or equal to requested line
for (int k = 0; k < entryCount && !done; k++) {
JVMDI_line_number_entry& entry = table[k];
if (entry.line_number >= saCmdBkptLineNumber &&
(!gotOne || entry.line_number < targetLineNumber)) {
gotOne = true;
targetClass = clazz;
targetMethod = m;
targetLocation = entry.start_location;
targetLineNumber = entry.line_number;
done = (targetLineNumber == saCmdBkptLineNumber);
}
}
} else if (lnRes != JVMDI_ERROR_ABSENT_INFORMATION) {
reportErrorToSA("Unexpected error %d while fetching line number table", lnRes);
return;
}
}
}
}
} else if (sfnRes != JVMDI_ERROR_ABSENT_INFORMATION) {
reportErrorToSA("Unexpected error %d while fetching source file name", sfnRes);
return;
}
}
bool wasSet = true;
if (gotOne) {
// Really toggle this breakpoint
jvmdiError bpRes;
bpRes = jvmdi->SetBreakpoint(targetClass, targetMethod, targetLocation);
if (bpRes == JVMDI_ERROR_DUPLICATE) {
bpRes = jvmdi->ClearBreakpoint(targetClass, targetMethod, targetLocation);
wasSet = false;
}
if (bpRes != JVMDI_ERROR_NONE) {
reportErrorToSA("Unexpected error %d while setting or clearing breakpoint at bci %d, line %d",
bpRes, targetLocation, targetLineNumber);
return;
}
} else {
saCmdBkptResWasError = 0;
reportErrorToSA("No debug information found covering this line");
return;
}
// Provide result
saCmdBkptResLineNumber = targetLineNumber;
saCmdBkptResBCI = targetLocation;
saCmdBkptResWasSet = (wasSet ? 1 : 0);
{
char* methodName;
char* methodSig;
if (jvmdi->GetMethodName(targetClass, targetMethod, &methodName, &methodSig)
== JVMDI_ERROR_NONE) {
JvmdiDeallocator mnd(methodName);
JvmdiDeallocator msd(methodSig);
strncpy(saCmdBkptResMethodName, methodName, SA_CMD_BUF_SIZE);
strncpy(saCmdBkptResMethodSig, methodSig, SA_CMD_BUF_SIZE);
} else {
strncpy(saCmdBkptResMethodName, "<error>", SA_CMD_BUF_SIZE);
strncpy(saCmdBkptResMethodSig, "<error>", SA_CMD_BUF_SIZE);
}
}
break;
}
default:
reportErrorToSA("Command %d not yet supported", saCmdType);
return;
}
// Successful command execution
saCmdResult = 0;
saCmdPending = 0;
}