/* check if splitting edge i of k is ok */
int chkspl(pMesh mesh,pSol met,int k,int i) {
pTria pt,pt1;
pPoint ppt;
pGeom go;
Bezier b;
double s,uv[2],o[3],no[3],to[3];
int *adja,jel,ip,ier;
char i1,i2,j,jj,j2;
if ( mesh->ng > mesh->ngmax-2 ) return(0);
pt = &mesh->tria[k];
i1 = inxt[i];
i2 = iprv[i];
if ( MS_SIN(pt->tag[i1]) || MS_SIN(pt->tag[i2]) ) return(0);
adja = &mesh->adja[3*(k-1)+1];
jel = adja[i] / 3;
if ( jel ) {
j = adja[i] % 3;
jj = inxt[j];
j2 = iprv[j];
pt1 = &mesh->tria[jel];
if ( MS_SIN(pt1->tag[jj]) || MS_SIN(pt1->tag[j2]) ) return(0);
}
ier = bezierCP(mesh,k,&b);
assert(ier);
/* create midedge point */
uv[0] = 0.5;
uv[1] = 0.5;
if (i == 1) uv[0] = 0.0;
else if ( i == 2 ) uv[1] = 0.0;
ier = bezierInt(&b,uv,o,no,to);
assert(ier);
ip = newPt(mesh,o,MS_EDG(pt->tag[i]) ? to : no);
assert(ip);
if ( MS_EDG(pt->tag[i]) ) {
++mesh->ng;
ppt = &mesh->point[ip];
ppt->ig = mesh->ng;
go = &mesh->geom[mesh->ng];
memcpy(go->n1,no,3*sizeof(double));
}
s = 0.5;
intmet(mesh,met,k,i,ip,s);
return(ip);
}
void JSpline::GeneratePixels()
{
float x, y;
float inc = SMALL_NUMBER;
mPixels.clear();
x = mMidPoints[1].x;
y = mMidPoints[1].y;
Point newPt(x, y);
Point extraPt;
mPixels.push_back(newPt);
for (int n=0; n < (int)mMidPoints.size()-3; n++)
{
float t = inc;
while (t <= 1.0f)
{
PointOnCurve(newPt, t, mMidPoints[n], mMidPoints[n+1], mMidPoints[n+2], mMidPoints[n+3]);
float dx = newPt.x-x;
float dy = newPt.y-y;
float dist = sqrtf(dx*dx + dy*dy);
if (dist >= MID_POINT_THRESHOLD)
{
//
//extraPt.x = (newPt.x+x)/2;
//extraPt.y = (newPt.y+y)/2;
//mPixels.push_back(extraPt);
//
mPixels.push_back(newPt);
x = newPt.x;
y = newPt.y;
}
t += inc;
}
}
mCount = mPixels.size();
}
particle *floodfill(matrix *mat, int id, int x, int y){
particle *newparticle;
int *current_id;
if((current_id = calloc(1,sizeof(int)))==NULL){
fprintf(stderr, "Out of memory error\n");
exit(1);
}
*current_id = 1;
newparticle = newParticle(id);
pt *newpt;
newpt = newPt(*current_id,x,y);
newparticle->spt = newpt;
newparticle->ept = newpt;
newparticle->size++;
floodfill_work(mat,newparticle,x,y,current_id);
free(current_id);
return newparticle;
}
void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
{
if (depth > m_manifoldPtr->GetContactBreakingTreshold())
return;
SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
SimdVector3 localA = m_transAInv(pointA );
SimdVector3 localB = m_transBInv(pointInWorld);
ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
if (insertIndex >= 0)
{
m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
} else
{
m_manifoldPtr->AddManifoldPoint(newPt);
}
}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
btVector3 localA;
btVector3 localB;
if (isSwapped)
{
localA = m_rootTransB.invXform(pointA );
localB = m_rootTransA.invXform(pointInWorld);
} else
{
localA = m_rootTransA.invXform(pointA );
localB = m_rootTransB.invXform(pointInWorld);
}
btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
//BP mod, store contact triangles.
if (isSwapped)
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
} else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//experimental feature info, for per-triangle material etc.
btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
}
void floodfill_work(matrix *mat, particle *part, int x, int y, int *id){
if(mat->vals[x][y]==0){
return;
}
pt *newpt;
(*id)++;
newpt = newPt(*id,x,y);
pushPt(newpt, part);
part->size++;
mat->vals[x][y] = 0;
if(!(x==0) && !(y==0)){
floodfill_work( mat, part, x-1, y-1, id);
}
if(!(x==mat->width-1) && !(y==mat->height-1)){
floodfill_work( mat, part, x+1, y+1, id);
}
if(!(x==0) && !(y==mat->height-1)){
floodfill_work( mat, part, x-1, y+1, id);
}
if(!(x==(mat->width-1)) && !(y==0)){
floodfill_work( mat, part, x+1, y-1, id);
}
if(!(x==0)){
floodfill_work( mat, part,x-1, y, id);
}
if(!(x==mat->width-1)){
floodfill_work( mat, part,x+1,y, id);
}
if(!(y==0)){
floodfill_work( mat, part,x,y-1, id);
}
if(!(y==mat->height-1)){
floodfill_work( mat, part,x, y+1, id);
}
return;
}
vector<margBlob> margBlobInterpolator::interpolate(margBlob& blob1, margBlob& blob2) {
// Calculate path's function
int delta_x = (blob2.centroid.x-blob1.centroid.x);
int delta_y = (blob2.centroid.y-blob1.centroid.y);
double path_angle = (double)delta_y/(double)delta_x;
double path_orig = (double)blob1.centroid.y - ((double)path_angle * (double)blob1.centroid.x);
int numSteps = ofDist(blob1.centroid.x, blob1.centroid.y, blob2.centroid.x, blob2.centroid.y);
double step_x = (double)delta_x/(double)numSteps;
double step_y = (double)delta_y/(double)numSteps;
vector<margBlob> interBlobs(numSteps+2);
vector< vector<float> > transPts;
// Normalize shape coordinates
vector<ofPoint> blob1_normPts(blob1.nPts);
vector<ofPoint> blob2_normPts(blob2.nPts);
for(int p = 0; p < blob1.nPts; p++) {
blob1_normPts[p] = normCart(blob1.pts[p].x, blob1.pts[p].y, blob1.centroid.x, blob1.centroid.y);
}
for(int p = 0; p < blob2.nPts; p++) {
blob2_normPts[p] = normCart(blob2.pts[p].x, blob2.pts[p].y, blob2.centroid.x, blob2.centroid.y);
}
// Compare number of vertices and trigger the appropriate methods in each case
// If both shapes have the same number of vertices
if(blob1.nPts == blob2.nPts) {
// Set transformation paths
for(int p = 0; p < blob1.nPts; p++) {
int delta_pt_x = round(blob2_normPts[p].x-blob1_normPts[p].x);
int delta_pt_y = round(blob2_normPts[p].y-blob1_normPts[p].y);
float step_pt_x= (float)((double)delta_pt_x/(double)numSteps);
float step_pt_y= (float)((double)delta_pt_y/(double)numSteps);
float curPts[] = {blob1_normPts[p].x, blob1_normPts[p].y, step_pt_x, step_pt_y};
vector<float> newPt (curPts, curPts + sizeof(curPts) / sizeof(float));
transPts.push_back(newPt);
newPt.clear();
}
}
else if (blob1.nPts > blob2.nPts) {
int diff = blob1.nPts - blob2.nPts;
int aver_diff = 0;
int amt_aver = 1;
if (diff <= blob2.nPts) {
aver_diff = floor((float)blob2.nPts / (float)diff);
}
else {
aver_diff = 1;
amt_aver = ceil((float)diff / (float)blob2.nPts);
}
int addedExtraPts = 0;
for(int p = 0; p < blob2.nPts; p++) {
int curSh1Pt = p + addedExtraPts;
int curSh2Pt = p;
if(curSh2Pt >= blob2.nPts) { curSh2Pt -= blob2.nPts; }
int prevSh2Pt = curSh2Pt - 1;
if(prevSh2Pt < 0) { prevSh2Pt += blob2.nPts; }
if((p+1) % aver_diff == 0 && addedExtraPts < diff) {
for(int amt = 0; amt < amt_aver; amt ++) {
if (addedExtraPts < diff) {
int final_pt_x = round(((blob2_normPts[curSh2Pt].x - blob2_normPts[prevSh2Pt].x) / 2.0) + blob2_normPts[prevSh2Pt].x); //Stablish final coord for extra point in shape 1
int final_pt_y = round(((blob2_normPts[curSh2Pt].y - blob2_normPts[prevSh2Pt].y) / 2.0) + blob2_normPts[prevSh2Pt].y); //as the middle of one of the sides of shape 2
int delta_pt_x = round(final_pt_x - blob1_normPts[curSh1Pt].x);
int delta_pt_y = round(final_pt_y - blob1_normPts[curSh1Pt].y);
float step_pt_x = (float)((double)delta_pt_x/(double)numSteps);
float step_pt_y = (float)((double)delta_pt_y/(double)numSteps);
float curPts[] = {blob1_normPts[curSh1Pt].x, blob1_normPts[curSh1Pt].y, step_pt_x, step_pt_y};
vector<float> newPt (curPts, curPts + sizeof(curPts) / sizeof(float));
transPts.push_back(newPt);
newPt.clear();
addedExtraPts ++;
curSh1Pt ++;
}
}
}
if (curSh1Pt < blob1.nPts) {
int delta_pt_x = round(blob2_normPts[curSh2Pt].x-blob1_normPts[curSh1Pt].x);
int delta_pt_y = round(blob2_normPts[curSh2Pt].y-blob1_normPts[curSh1Pt].y);
float step_pt_x= (float)((double)delta_pt_x/(double)numSteps);
float step_pt_y= (float)((double)delta_pt_y/(double)numSteps);
float curPts[] = {blob1_normPts[curSh1Pt].x, blob1_normPts[curSh1Pt].y, step_pt_x, step_pt_y};
vector<float> newPt (curPts, curPts + sizeof(curPts) / sizeof(float));
transPts.push_back(newPt);
newPt.clear();
}
}
}
else if (blob1.nPts < blob2.nPts) {
int diff = blob2.nPts - blob1.nPts;
int aver_diff = 0;
int amt_aver = 1;
if (diff <= blob1.nPts) {
aver_diff = floor((float)blob1.nPts / (float)diff);
}
else {
aver_diff = 1;
amt_aver = ceil((float)diff / (float)blob1.nPts);
}
int removedExtraPts = 0;
for(int p = 0; p < blob1.nPts; p++) {
int curSh2Pt = p + removedExtraPts;
if(curSh2Pt >= blob2.nPts) { curSh2Pt -= blob2.nPts;}
int curSh1Pt = p;
int prevSh1Pt = curSh1Pt - 1;
if(prevSh1Pt < 0) { prevSh1Pt += blob1.nPts;}
if((p+1) % aver_diff == 0 && removedExtraPts < diff) {
for(int amt = 0; amt < amt_aver; amt++) {
if (removedExtraPts < diff) {
int init_pt_x = round((blob1_normPts[curSh1Pt].x - blob1_normPts[prevSh1Pt].x)/2.0 + blob1_normPts[prevSh1Pt].x); //Stablish initial coord for extra point of shape 2
int init_pt_y = round((blob1_normPts[curSh1Pt].y - blob1_normPts[prevSh1Pt].y)/2.0 + blob1_normPts[prevSh1Pt].y); //as the middle of one of the sides of shape 1
int delta_pt_x = round(blob2_normPts[curSh2Pt].x-init_pt_x);
int delta_pt_y = round(blob2_normPts[curSh2Pt].y-init_pt_y);
float step_pt_x= (float)((double)delta_pt_x/(double)numSteps);
float step_pt_y= (float)((double)delta_pt_y/(double)numSteps);
float curPts[] = {init_pt_x, init_pt_y, step_pt_x, step_pt_y};
vector<float> newPt (curPts, curPts + sizeof(curPts) / sizeof(float));
transPts.push_back(newPt);
newPt.clear();
removedExtraPts ++;
curSh2Pt++;
if(curSh2Pt >= blob2.pts.size()) { curSh2Pt -= blob2.pts.size();}
}
}
}
if (curSh2Pt < blob2.nPts) {
int delta_pt_x = round(blob2_normPts[curSh2Pt].x-blob1_normPts[curSh1Pt].x);
int delta_pt_y = round(blob2_normPts[curSh2Pt].y-blob1_normPts[curSh1Pt].y);
float step_pt_x= (float)((double)delta_pt_x/(double)numSteps);
float step_pt_y= (float)((double)delta_pt_y/(double)numSteps);
float curPts[] = {blob1_normPts[curSh1Pt].x, blob1_normPts[curSh1Pt].y, step_pt_x, step_pt_y};
vector<float> newPt (curPts, curPts + sizeof(curPts) / sizeof(float));
transPts.push_back(newPt);
newPt.clear();
}
}
}
float exposureLev = 1;
//if (numSteps > 0) { exposureLev = 1 / numSteps; }
blob1.exposure = exposureLev;
blob2.exposure = exposureLev;
interBlobs.push_back(blob1);
for(int i = 0; i < numSteps; i++) {
margBlob newBlob;
newBlob.centroid.x=round(blob1.centroid.x+(i*(float)step_x));
newBlob.centroid.y=round(blob1.centroid.y+(i*(float)step_y));
newBlob.exposure = exposureLev;
float minX = -1;
float minY = -1;
float maxX = -1;
float maxY = -1;
int nTransPts = transPts.size();
for(int p = 0; p < nTransPts; p++) {
float x = newBlob.centroid.x + transPts[p][0] + (i * transPts[p][2]);
float y = newBlob.centroid.y + transPts[p][1] + (i * transPts[p][3]);
if (x < minX || minX == -1) {
minX = x;
}
if (x > maxX || maxX == -1) {
maxX = x;
}
if (y < minY || minY == -1) {
minY = y;
}
if (y > maxY || maxY == -1) {
maxY = y;
}
newBlob.pts.push_back(ofPoint(x, y));
}
newBlob.boundingRect.x = minX;
newBlob.boundingRect.y = minY;
newBlob.boundingRect.width = (maxX - minX);
newBlob.boundingRect.height = (maxY - minY);
interBlobs.push_back(newBlob);
}
interBlobs.push_back(blob2);
blob1_normPts.clear();
blob2_normPts.clear();
transPts.clear();
return interBlobs;
}
void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
assert(m_manifoldPtr);
//order in manifold needs to match
if (depth > m_manifoldPtr->getContactBreakingThreshold())
return;
bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
btVector3 localA;
btVector3 localB;
if (isSwapped)
{
localA = m_rootTransB.invXform(pointA );
localB = m_rootTransA.invXform(pointInWorld);
} else
{
localA = m_rootTransA.invXform(pointA );
localB = m_rootTransB.invXform(pointInWorld);
}
btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
//BP mod, store contact triangles.
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
///todo, check this for any side effects
if (insertIndex >= 0)
{
//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
} else
{
m_manifoldPtr->AddManifoldPoint(newPt);
}
//User can override friction and/or restitution
if (gContactAddedCallback &&
//and if either of the two bodies requires custom material
((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
(m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
{
//experimental feature info, for per-triangle material etc.
btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
(*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
}
}
void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
{
btAssert(m_manifoldPtr);
//order in manifold needs to match
if (depth > m_manifoldPtr->getContactBreakingThreshold())
// if (depth > m_manifoldPtr->getContactProcessingThreshold())
return;
bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
btVector3 localA;
btVector3 localB;
if (isSwapped)
{
localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
} else
{
localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
}
btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2);
//BP mod, store contact triangles.
if (isSwapped)
{
newPt.m_partId0 = m_partId1;
newPt.m_partId1 = m_partId0;
newPt.m_index0 = m_index1;
newPt.m_index1 = m_index0;
} else
{
newPt.m_partId0 = m_partId0;
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
}
//printf("depth=%f\n", depth);
///@todo, check this for any side effects
if (insertIndex >= 0)
{
//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
m_manifoldPtr->replaceContactPoint(newPt, insertIndex);
} else
{
insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
}
//User can override friction and/or restitution
// DrChat: Removed for multithreading version
/*
if (gContactAddedCallback &&
//and if either of the two bodies requires custom material
((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
{
//experimental feature info, for per-triangle material etc.
const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
}
*/
}
/* analyze triangles and split if needed */
static int anaelt(pMesh mesh,pSol met,char typchk) {
pTria pt;
pPoint ppt,p1,p2;
Hash hash;
Bezier pb;
pGeom go;
double s,o[3],no[3],to[3],dd,len;
int vx[3],i,j,ip,ip1,ip2,ier,k,ns,nc,nt;
char i1,i2;
static double uv[3][2] = { {0.5,0.5}, {0.,0.5}, {0.5,0.} };
hashNew(&hash,mesh->np);
ns = 0;
s = 0.5;
for (k=1; k<=mesh->nt; k++) {
pt = &mesh->tria[k];
if ( !MS_EOK(pt) || pt->ref < 0 ) continue;
if ( MS_SIN(pt->tag[0]) || MS_SIN(pt->tag[1]) || MS_SIN(pt->tag[2]) ) continue;
/* check element cut */
pt->flag = 0;
if ( typchk == 1 ) {
if ( !chkedg(mesh,k) ) continue;
}
else if ( typchk == 2 ) {
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = iprv[i];
len = lenedg(mesh,met,pt->v[i1],pt->v[i2],0);
if ( len > LLONG ) MS_SET(pt->flag,i);
}
if ( !pt->flag ) continue;
}
ns++;
/* geometric support */
ier = bezierCP(mesh,k,&pb);
assert(ier);
/* scan edges to split */
for (i=0; i<3; i++) {
if ( !MS_GET(pt->flag,i) ) continue;
i1 = inxt[i];
i2 = iprv[i];
ip1 = pt->v[i1];
ip2 = pt->v[i2];
ip = hashGet(&hash,ip1,ip2);
if ( !MS_EDG(pt->tag[i]) && ip > 0 ) continue;
/* new point along edge */
ier = bezierInt(&pb,uv[i],o,no,to);
if ( !ip ) {
ip = newPt(mesh,o,MS_EDG(pt->tag[i]) ? to : no);
assert(ip);
hashEdge(&hash,ip1,ip2,ip);
p1 = &mesh->point[ip1];
p2 = &mesh->point[ip2];
ppt = &mesh->point[ip];
if ( MS_EDG(pt->tag[i]) ) {
++mesh->ng;
assert(mesh->ng < mesh->ngmax);
ppt->tag = pt->tag[i];
if ( p1->ref == pt->edg[i] || p2->ref == pt->edg[i] )
ppt->ref = pt->edg[i];
ppt->ig = mesh->ng;
go = &mesh->geom[mesh->ng];
memcpy(go->n1,no,3*sizeof(double));
dd = go->n1[0]*ppt->n[0] + go->n1[1]*ppt->n[1] + go->n1[2]*ppt->n[2];
ppt->n[0] -= dd*go->n1[0];
ppt->n[1] -= dd*go->n1[1];
ppt->n[2] -= dd*go->n1[2];
dd = ppt->n[0]*ppt->n[0] + ppt->n[1]*ppt->n[1] + ppt->n[2]*ppt->n[2];
if ( dd > EPSD2 ) {
dd = 1.0 / sqrt(dd);
ppt->n[0] *= dd;
ppt->n[1] *= dd;
ppt->n[2] *= dd;
}
}
if ( met->m ) {
if ( typchk == 1 )
intmet33(mesh,met,ip1,ip2,ip,s);
else
intmet(mesh,met,k,i,ip,s);
}
}
else if ( pt->tag[i] & MS_GEO ) {
ppt = &mesh->point[ip];
go = &mesh->geom[ppt->ig];
memcpy(go->n2,no,3*sizeof(double));
/* a computation of the tangent with respect to these two normals is possible */
ppt->n[0] = go->n1[1]*go->n2[2] - go->n1[2]*go->n2[1];
ppt->n[1] = go->n1[2]*go->n2[0] - go->n1[0]*go->n2[2];
ppt->n[2] = go->n1[0]*go->n2[1] - go->n1[1]*go->n2[0];
dd = ppt->n[0]*ppt->n[0] + ppt->n[1]*ppt->n[1] + ppt->n[2]*ppt->n[2];
if ( dd > EPSD2 ) {
dd = 1.0 / sqrt(dd);
ppt->n[0] *= dd;
ppt->n[1] *= dd;
ppt->n[2] *= dd;
}
}
}
}
if ( !ns ) {
free(hash.item);
return(ns);
}
/* step 2. checking if split by adjacent */
for (k=1; k<=mesh->nt; k++) {
pt = &mesh->tria[k];
if ( !MS_EOK(pt) || pt->ref < 0 ) continue;
else if ( pt->flag == 7 ) continue;
/* geometric support */
ier = bezierCP(mesh,k,&pb);
assert(ier);
nc = 0;
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = inxt[i1];
if ( !MS_GET(pt->flag,i) && !MS_SIN(pt->tag[i]) ) {
ip = hashGet(&hash,pt->v[i1],pt->v[i2]);
if ( ip > 0 ) {
MS_SET(pt->flag,i);
nc++;
if ( pt->tag[i] & MS_GEO ) {
/* new point along edge */
ier = bezierInt(&pb,uv[i],o,no,to);
assert(ier);
ppt = &mesh->point[ip];
go = &mesh->geom[ppt->ig];
memcpy(go->n2,no,3*sizeof(double));
/* a computation of the tangent with respect to these two normals is possible */
ppt->n[0] = go->n1[1]*go->n2[2] - go->n1[2]*go->n2[1];
ppt->n[1] = go->n1[2]*go->n2[0] - go->n1[0]*go->n2[2];
ppt->n[2] = go->n1[0]*go->n2[1] - go->n1[1]*go->n2[0];
dd = ppt->n[0]*ppt->n[0] + ppt->n[1]*ppt->n[1] + ppt->n[2]*ppt->n[2];
if ( dd > EPSD2 ) {
dd = 1.0 / sqrt(dd);
ppt->n[0] *= dd;
ppt->n[1] *= dd;
ppt->n[2] *= dd;
}
}
}
}
}
if ( nc > 0 ) ++ns;
}
if ( info.ddebug && ns ) {
fprintf(stdout," %d analyzed %d proposed\n",mesh->nt,ns);
fflush(stdout);
}
/* step 3. splitting */
ns = 0;
nt = mesh->nt;
for (k=1; k<=nt; k++) {
pt = &mesh->tria[k];
if ( !MS_EOK(pt) || pt->ref < 0 ) continue;
else if ( pt->flag == 0 ) continue;
j = -1;
vx[0] = vx[1] = vx[2] = 0;
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = inxt[i1];
if ( MS_GET(pt->flag,i) ) {
vx[i] = hashGet(&hash,pt->v[i1],pt->v[i2]);
assert(vx[i]);
j = i;
}
}
if ( pt->flag == 1 || pt->flag == 2 || pt->flag == 4 ) {
ier = split1(mesh,met,k,j,vx);
assert(ier);
ns++;
}
else if ( pt->flag == 7 ) {
ier = split3(mesh,met,k,vx);
assert(ier);
ns++;
}
else {
ier = split2(mesh,met,k,vx);
assert(ier);
ns++;
}
}
if ( (info.ddebug || abs(info.imprim) > 5) && ns > 0 )
fprintf(stdout," %7d splitted\n",ns);
free(hash.item);
return(ns);
}
///return true if it requires a dma transfer back
bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
const btVector3& pointInWorld,
float depth,
btPersistentManifold* manifoldPtr,
btTransform& transA,
btTransform& transB,
btScalar combinedFriction,
btScalar combinedRestitution,
bool isSwapped)
{
// float contactTreshold = manifoldPtr->getContactBreakingThreshold();
//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: contactTreshold %f\n",contactTreshold);
#endif //DEBUG_SPU_COLLISION_DETECTION
//if (depth > manifoldPtr->getContactBreakingThreshold())
// return false;
if (depth > manifoldPtr->getContactProcessingThreshold())
return false;
btVector3 pointA;
btVector3 localA;
btVector3 localB;
btVector3 normal;
if (isSwapped)
{
normal = normalOnBInWorld * -1;
pointA = pointInWorld + normal * depth;
localA = transA.invXform(pointA );
localB = transB.invXform(pointInWorld);
}
else
{
normal = normalOnBInWorld;
pointA = pointInWorld + normal * depth;
localA = transA.invXform(pointA );
localB = transB.invXform(pointInWorld);
}
btManifoldPoint newPt(localA,localB,normal,depth);
newPt.m_positionWorldOnA = pointA;
newPt.m_positionWorldOnB = pointInWorld;
newPt.m_combinedFriction = combinedFriction;
newPt.m_combinedRestitution = combinedRestitution;
int insertIndex = manifoldPtr->getCacheEntry(newPt);
if (insertIndex >= 0)
{
// we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc)
manifoldPtr->replaceContactPoint(newPt,insertIndex);
return true;
} else
{
/*
///@todo: SPU callbacks, either immediate (local on the SPU), or deferred
//User can override friction and/or restitution
if (gContactAddedCallback &&
//and if either of the two bodies requires custom material
((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
(m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
{
//experimental feature info, for per-triangle material etc.
(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
}
*/
manifoldPtr->addManifoldPoint(newPt);
return true;
}
return false;
}
//create the mesh for edges based on variable size from SizingFunction (var)
void EdgeMesher::VariableMeshing(ModelEnt *ent, int &num_edges, std::vector<double> &coords)
{
double umin, umax, measure;
(void) measure;
// because of that, keep track of the first node position and last node position
// first node position does not change, but the last node position do change
// coords will contain all nodes, including umax in Urecord!
SizingFunction *sf = mk_core()->sizing_function(ent->sizing_function_index());
//get the u range for the edge
iGeom::EntityHandle edge = ent->geom_handle();
iGeom::Error gerr = ent->igeom_instance() ->getEntURange(edge, umin, umax);
IBERRCHK(gerr, "Trouble get parameter range for edge.");
if (umin == umax) throw Error(MK_BAD_GEOMETRIC_EVALUATION, "Edge evaluated to some parameter umax and umin.");
//get the arc length
measure = ent -> measure();
// start advancing for each edge mesh, from the first point position
double currentPar = umin;
double currentPosition[3];
gerr = ent->igeom_instance() ->getEntUtoXYZ(edge, umin, currentPosition[0],
currentPosition[1], currentPosition[2] );
double endPoint[3];
gerr = ent->igeom_instance() ->getEntUtoXYZ(edge, umax, endPoint[0],
endPoint[1], endPoint[2] );
Vector<3> endpt(endPoint);
double targetSize = sf->size(currentPosition);
double startSize = targetSize;
double endSize = sf->size(endPoint);
// advance u such that the next point is at "currentSize" distance
// or close to it
// try first with a u that is coming from the (umax-umin)/number of edges
double deltaU = (umax-umin)/num_edges;
//coords.clear(); we do not want to clear, as the first node is still fine
std::vector<double> URecord; //record the values for u; we may have to adjust all
// of them accordingly, and even add one more if we have some evenify problems.
// keep in mind that size is just a suggestion, number of intervals is more important for
// paver mesher
Vector<3> pt(currentPosition);
//bool notDone = true;
double prevU = umin;
while (currentPar + 1.1*deltaU < umax)
{
// do some binary search; actually, better, do Newton-Raphson, which should converge
// faster
//
prevU = currentPar;
currentPar += deltaU;
// adjust current par, such that
double point[3];
gerr=ent->igeom_instance()->getEntUtoXYZ(edge, currentPar, point[0], point[1], point[2] );
IBERRCHK(gerr, "Trouble getting position at parameter u.");
Vector<3> ptCandidate(point);
double compSize = length(ptCandidate-pt);
int nit = 0;
while ( (fabs(1.-compSize/targetSize)> 0.02 ) && (nit < 5))// 2% of the target size
{
// do Newton iteration
double tangent[3];
gerr=ent->igeom_instance() ->getEntTgntU(edge, currentPar, tangent[0], tangent[1], tangent[2] );
IBERRCHK(gerr, "Trouble getting tangent at parameter u.");
Vector<3> tang(tangent);
double dldu = 1./compSize * ((ptCandidate-pt )%tang);
nit++;// increase iteration count
if (dldu!=0.)
{
double deu= (targetSize-compSize)/dldu;
currentPar+=deu;
if (prevU>currentPar)
{
break; // something is wrong
}
if (umax < currentPar)
{
currentPar = umax;
break;
}
ent->igeom_instance()->getEntUtoXYZ(edge, currentPar, point[0], point[1], point[2]);
Vector<3> newPt(point);
compSize = length(newPt-pt);
ptCandidate = newPt;
}
}
// we have found an acceptable point/param
URecord.push_back(currentPar);
deltaU = currentPar-prevU;// should be greater than 0
pt = ptCandidate;
targetSize = sf->size(pt.data());// a new target size, at the current point
}
// when we are out of here, we need to adjust the URecords, to be more nicely
// distributed; also, look at the evenify again
int sizeU = (int)URecord.size();
if ((sizeU%2==0) && ent->constrain_even() )
{
// add one more
if (sizeU==0)
{
// just add one in the middle, and call it done
URecord.push_back( (umin+umax)/2);
}
else
{
//at least 2 (maybe 4)
double lastDelta = URecord[sizeU-1]-URecord[sizeU-2];
URecord.push_back(URecord[sizeU-1]+lastDelta );
}
}
// now, we have to redistribute, such as the last 2 deltas are about the same
// so, we should have after a little work,
// umin, umin+c*(URecord[0]-umin), ... umin+c*(URecord[size-1]-umin), umax
// what we have now is
// umin, URecord[0], ... ,URecord[size-1], and umax could be even outside or inside
// keep the last sizes equal
// umin+c(UR[size-2]-umin) + umax = 2*( umin+c*(UR[size-1]-umin))
// c ( 2*(UR[size-1]-umin) -(UR[size-2]-umin) ) = umax - umin
// c ( 2*UR[size-1] - UR[size-2] - umin ) = umax - umin
// c = (umax-umin)/( 2*UR[size-1] - UR[size-2] - umin)
sizeU = (int)URecord.size();// it may be bigger by one than the last time
if (sizeU == 0)
{
// nothing to do, only one edge to generate
}
else if (sizeU == 1)
{
// put it according to the sizes at ends, and assume a nice variation for u
// (u-umin) / (umax-u) = startSize / endSize
// (u-umin)*endSize = (umax-u) * startSize
// u(endSize+startSize)=(umax*startSize+umin*endSize)
URecord[0] = (umax*startSize+umin*endSize)/(startSize+endSize);
}
else // sizeU>=2, so we can spread the param a little more, assuming nice
// uniform mapping
{
double c = (umax-umin)/( 2*URecord[sizeU-1] - URecord[sizeU-2] - umin);
for (int i=0; i<sizeU; i++)
URecord[i] = umin + c*(URecord[i] -umin);// some spreading out
}
// now, we can finally get the points for each U, U's should be spread out nicely
URecord.push_back(umax); // just add the last u, for the end point
//
sizeU = (int) URecord.size(); // new size, after pushing the last u, umax
num_edges = sizeU;// this is the new number of edges; the last one will be the end point
// of the edge, corresponding to umax
coords.resize(3*sizeU+3);
// we already know that at i=0 is the first node, start vertex of edge
// the rest will be computed from u
// even the last one, which is an overkill
for (int i = 1; i <= num_edges; i++)
{
double u = URecord[i-1];
gerr = ent->igeom_instance()->getEntUtoXYZ(edge, u, coords[3*i], coords[3*i+1], coords[3*i+2]);
IBERRCHK(gerr, "Trouble getting U from XYZ along the edge.");
}
return;
}
///return true if it requires a dma transfer back
bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
const btVector3& pointInWorld,
float depth,
btPersistentManifold* manifoldPtr,
btTransform& transA,
btTransform& transB,
btScalar combinedFriction,
btScalar combinedRestitution,
bool isSwapped)
{
float contactTreshold = manifoldPtr->getContactBreakingThreshold();
//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: contactTreshold %f\n",contactTreshold);
#endif //DEBUG_SPU_COLLISION_DETECTION
if (depth > manifoldPtr->getContactBreakingThreshold())
return false;
//provide inverses or just calculate?
btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform;
btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform;
btVector3 pointA;
btVector3 localA;
btVector3 localB;
btVector3 normal;
if (isSwapped)
{
normal = normalOnBInWorld * -1;
pointA = pointInWorld + normal * depth;
localA = transAInv(pointA );
localB = transBInv(pointInWorld);
/*localA = transBInv(pointA );
localB = transAInv(pointInWorld);*/
}
else
{
normal = normalOnBInWorld;
pointA = pointInWorld + normal * depth;
localA = transAInv(pointA );
localB = transBInv(pointInWorld);
}
btManifoldPoint newPt(localA,localB,normal,depth);
int insertIndex = manifoldPtr->getCacheEntry(newPt);
if (insertIndex >= 0)
{
// manifoldPtr->replaceContactPoint(newPt,insertIndex);
// return true;
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: same contact detected, nothing done\n");
#endif //DEBUG_SPU_COLLISION_DETECTION
// This is not needed, just use the old info! saves a DMA transfer as well
} else
{
newPt.m_combinedFriction = combinedFriction;
newPt.m_combinedRestitution = combinedRestitution;
/*
///@todo: SPU callbacks, either immediate (local on the SPU), or deferred
//User can override friction and/or restitution
if (gContactAddedCallback &&
//and if either of the two bodies requires custom material
((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
(m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
{
//experimental feature info, for per-triangle material etc.
(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
}
*/
manifoldPtr->addManifoldPoint(newPt);
return true;
}
return false;
}
