void MeshManipulator::smoothSurface(const Ray * r)
{
VertexAdjacency adj = m_topo->getAdjacency(m_intersect->m_componentIdx);
Vector3F *p = &m_mesh->vertices()[m_intersect->m_componentIdx];
Vector3F d = adj.center() - *p;
*p += d * .7f;
Plane pl(m_intersect->m_hitN, m_intersect->m_hitP);
Vector3F hit;
float t;
if(!pl.rayIntersect(*r, hit, t, 1)) return;
d = hit - *p;
float minD = d.length();
float curD;
VertexAdjacency::VertexNeighbor *neighbor;
for(neighbor = adj.firstNeighbor(); !adj.isLastNeighbor(); neighbor = adj.nextNeighbor()) {
d = hit - *(neighbor->v->m_v);
curD = d.length();
if(curD < minD) {
minD = curD;
m_intersect->m_componentIdx = neighbor->v->getIndex();
}
}
}
Vector3F LinearSpline::getPositionAtTime(unsigned int length, unsigned int time)
{
float splineLength = 0;
std::vector<float> segmentLengths;
for (size_t i = 1; i < mNodes.size(); ++i) {
float segmentLength = (mNodes[i - 1]->position.value() - mNodes[i]->position.value()).length();
splineLength += segmentLength;
segmentLengths.push_back(splineLength);
}
float step = splineLength / length;
float segment = time * step;
size_t segmentIndex = 0;
while (segment > segmentLengths[segmentIndex])
segmentIndex++;
float segmentRemains = segment - (segmentIndex > 0 ? segmentLengths[segmentIndex - 1] : 0);
Vector3F segmentToUse = mNodes[segmentIndex + 1]->position.value() - mNodes[segmentIndex]->position.value();
float segmentToUseLength = segmentToUse.length();
float segmentPercentage = segmentRemains / segmentToUseLength;
Vector3F segmentFraction = segmentToUse * segmentPercentage;
return mNodes[segmentIndex]->position.value() + segmentFraction;
}
void BaseView::tumble(int dx, int dy, int portWidth)
{
Vector3F side = m_space.getSide();
Vector3F up = m_space.getUp();
Vector3F front = m_space.getFront();
Vector3F eye = m_space.getTranslation();
Vector3F toEye = eye - m_centerOfInterest;
float dist = toEye.length();
const float scaleing = dist * 2.f / (float)portWidth;
eye -= side * (float)dx * scaleing;
eye += up * (float)dy * scaleing;
toEye = eye - m_centerOfInterest;
toEye.normalize();
eye = m_centerOfInterest + toEye * dist;
m_space.setTranslation(eye);
front = toEye;
side = up.cross(front);
side.y = 0.f;
side.normalize();
up = front.cross(side);
up.normalize();
m_space.setOrientations(side, up, front);
m_invSpace = m_space;
m_invSpace.inverse();
}
void GjkContactSolver::penetration(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
resetSimplex(result->W);
const Vector3F r = result->rayDirection;
const Vector3F startP = Vector3F::Zero - result->rayDirection * 99.f;
Vector3F hitP = startP;
// from origin to startP
Vector3F v = hitP;
Vector3F w, p, pa, pb, localA, localB;
float lamda = 0.f;
float vdotw, vdotr;
int k = 0;
for(; k < 39; k++) {
vdotr = v.dot(r);
// SA-B(v)
pa = A.supportPoint(v, result->transformA, localA, result->margin);
pb = B.supportPoint(v.reversed(), result->transformB, localB, result->margin);
p = pa - pb;// + v.normal() * MARGIN_DISTANCE;
w = hitP - p;
vdotw = v.dot(w);
if(vdotw > 0.f) {
if(vdotr >= 0.f)
break;
lamda -= vdotw / vdotr;
hitP = startP + r * lamda;
}
addToSimplex(result->W, p, localB);
result->hasResult = 0;
result->distance = 1e9;
result->referencePoint = hitP;
closestOnSimplex(result);
v = hitP - result->closestPoint;
interpolatePointB(result);
if(v.length2() < TINY_VALUE) break;
result->separateAxis = v;
smallestSimplex(result);
}
result->distance = hitP.length();
result->separateAxis.normalize();
}
void BaseView::zoom(int dz, int portWidth)
{
Vector3F front = m_space.getFront();
Vector3F eye = m_space.getTranslation();
Vector3F toEye = eye - m_centerOfInterest;
const float dist = toEye.length();
const float fra = (float)dz/(float)portWidth * 7.f;
eye += front * dist * -fra;
if(fra > 0.f && dist < 10.f)
m_centerOfInterest += front * dist * -fra * 0.1f;
m_space.setTranslation(eye);
m_invSpace = m_space;
m_invSpace.inverse();
}
unsigned BaseMesh::closestVertex(unsigned idx, const Vector3F & px) const
{
unsigned *trii = &_indices[idx * 3];
float mag, minDist = 10e8;
unsigned vert = 0;
for(int i = 0; i < 3; i++) {
Vector3F v = _vertices[*trii] - px;
mag = v.length();
if(mag < minDist) {
minDist = mag;
vert = *trii;
}
trii++;
}
return vert;
}
unsigned PatchMesh::closestVertex(unsigned idx, const Vector3F & px) const
{
unsigned *qudi = &m_quadIndices[idx * 4];
float mag, minDist = 10e8;
unsigned vert = 0;
for(int i = 0; i < 4; i++) {
Vector3F v = getVertices()[*qudi] - px;
mag = v.length();
if(mag < minDist) {
minDist = mag;
vert = *qudi;
}
qudi++;
}
return vert;
}
void Vector3F::rotateAroundAxis(const Vector3F& axis, float theta)
{
if(theta==0) return;
Vector3F ori(x,y,z);
float l = ori.length();
ori.normalize();
Vector3F up = axis.cross(ori);
up.normalize();
Vector3F side = ori - axis*(axis.dot(ori));
up *=side.length();
ori += side*(cos(theta) - 1);
ori += up*sin(theta);
ori.normalize();
x = ori.x*l;
y = ori.y*l;
z = ori.z*l;
}
void GjkContactSolver::timeOfImpact(const PointSet & A, const PointSet & B, ContinuousCollisionContext * result)
{
result->hasContact = 0;
result->penetrateDepth = 0.f;
result->TOI = 0.f;
// std::cout<<"\nb test p"<<result->positionB;
// std::cout<<"\nb test v"<<result->linearVelocityB * 60.f;
// std::cout<<"\nb test w"<<result->angularVelocityB * 60.f;
const Vector3F relativeLinearVelocity = result->linearVelocityB - result->linearVelocityA;
// std::cout<<" velocityA "<<result->linearVelocityA.str();
// std::cout<<" velocityB "<<result->linearVelocityB.str();
// std::cout<<" relativeLinearVelocity "<<relativeLinearVelocity.str();
const float angularMotionSize = result->angularVelocityA.length() * A.angularMotionDisc()
+ result->angularVelocityB.length() * B.angularMotionDisc();
// no relative motion
if(relativeLinearVelocity.length() + angularMotionSize < TINY_VALUE)
return;
#ifdef DBG_DRAW
Vector3F lineB = result->positionB;
Vector3F lineE = lineB + relativeLinearVelocity;
glColor3f(0.f, .1f, .6f);
m_dbgDrawer->arrow(lineB, lineE);
lineB = result->positionA;
lineE = lineB - relativeLinearVelocity;
glColor3f(0.f, .1f, .6f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
ClosestTestContext separateIo;
separateIo.needContributes = 1;
separateIo.margin = 0.05f;
Vector3F separateN;
float distance, closeInSpeed;
float lastDistance = 0.f;
float dDistanceaLamda;
const Vector3F position0A = result->positionA;
const Vector3F position0B = result->positionB;
const Quaternion orientation0A = result->orientationA;
const Quaternion orientation0B = result->orientationB;
float lamda = 0.f;
float limitDeltaLamda, deltaLamda = 1.f;
float lastLamda = 0.f;
int k = 0;
for(; k < 32; k++) {
separateIo.transformA.setTranslation(position0A.progress(result->linearVelocityA, lamda));
Quaternion ra = orientation0A.progress(result->angularVelocityA, lamda);
ra.normalize();
separateIo.transformA.setRotation(ra);
separateIo.transformB.setTranslation(position0B.progress(result->linearVelocityB, lamda));
Quaternion rb = orientation0B.progress(result->angularVelocityB, lamda);
rb.normalize();
separateIo.transformB.setRotation(rb);
// std::cout<<"\nk "<<k;
// std::cout<<"\nb at p"<<separateIo.transformB.getTranslation();
// std::cout<<"\nmat"<<separateIo.transformB.str();
separateIo.referencePoint.setZero();
separateIo.distance = 1e9;
separateDistance(A, B, &separateIo);
if(separateIo.hasResult) {
if(k<1) {
std::cout<<" contact at t0 try zero margin\n";
separateIo.margin = 0.f;
separateIo.distance = 1e9;
separateDistance(A, B, &separateIo);
if(separateIo.hasResult) {
std::cout<<" penetrated\n";
result->hasContact = 0;
return;
}
result->contactPointB = separateIo.contactPointB;
distance = separateIo.separateAxis.length();
result->penetrateDepth = 0.1 - distance;
separateN = separateIo.separateAxis / distance;
#ifdef DBG_GJK_DRAW
lineB = separateIo.transformB.transform(separateIo.contactPointB);
lineE = lineB + separateN;
glColor3f(1.f, 0.f, 0.f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
break;
} else {
// std::cout<<" contact at "<<lamda;;
lamda = lastLamda;
break;
}
}
result->contactPointB = separateIo.contactPointB;
distance = separateIo.separateAxis.length();
if(distance < .001f) {
// std::cout<<" "<<k<<" close enough at "<<lamda<<"\n";
if(k<1) {
separateIo.margin = 0.f;
separateIo.distance = 1e9;
separateDistance(A, B, &separateIo);
result->contactPointB = separateIo.contactPointB;
distance = separateIo.separateAxis.length();
separateN = separateIo.separateAxis / distance;
}
break;
}
separateN = separateIo.separateAxis / distance;
#ifdef DBG_GJK_DRAW
lineB = separateIo.transformB.transform(separateIo.contactPointB);
lineE = lineB + separateN;
glColor3f(1.f, 0.f, 0.f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
dDistanceaLamda = (distance - lastDistance) / deltaLamda;
lastDistance = distance;
// std::cout<<" sep ax "<<separateIo.separateAxis.str();
// std::cout<<" dist "<<distance;
// std::cout<<" sep n "<<separateN.str();
closeInSpeed = relativeLinearVelocity.dot(separateN);
// std::cout<<" closeInSpeed "<<closeInSpeed;
if(closeInSpeed + angularMotionSize < 0.f) {
// std::cout<<"go apart at time "<<lamda<<"\n";
return;
}
deltaLamda = distance / (closeInSpeed + angularMotionSize);
if(dDistanceaLamda < 0.f) {
limitDeltaLamda = -.59f * lastDistance / dDistanceaLamda;
if(deltaLamda > limitDeltaLamda) {
deltaLamda = limitDeltaLamda;
// std::cout<<" limit delta lamda "<<deltaLamda<<"\n";
}
}
lastLamda = lamda;
lamda += deltaLamda;
if(lamda < 0.f) {
// std::cout<<"lamda < 0\n";
return;
}
if(lamda > 1.f) {
// std::cout<<"lamda > 1\n";
return;
}
}
result->hasContact = 1;
result->TOI = lamda;
result->contactNormal = separateN.reversed();
// std::cout<<" v.n "<<result->contactNormal.dot(relativeLinearVelocity);
// std::cout<<" TOI "<<result->TOI;
// result->contactNormal.verbose(" N");
// relativeLinearVelocity.verbose(" Vrel");
#ifdef DBG_DRAW
lineB = separateIo.transformB.transform(separateIo.contactPointB);
lineE = lineB + result->contactNormal;
glColor3f(.2f, 1.f, .1f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
}