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();
}
/// mean normal + from center
void Sculptor::inflatePoints()
{
Vector3F nor = m_active->meanNormal;
Array<int, VertexP> * vs = m_active->vertices;
float wei, round;
vs->begin();
while(!vs->end()) {
VertexP * l = vs->value();
wei = *l->index->t4;
const Vector3F p0(*(l->index->t1));
Vector3F pn = *l->index->t2;
/// blow outwards
if(pn.dot(nor) < 0.f) pn.reverse();
round = cos(p0.distanceTo(m_active->meanPosition) / selectRadius() * 1.5f );
pn *= round;
pn += nor * round;
*(l->index->t1) += pn * wei * m_strength * 0.1f;
m_tree->displace(l, *(l->index->t1), p0);
vs->next();
}
smoothPoints(0.4f);
}
void Plane::set(const Vector3F & nor, const Vector3F & pop)
{
Vector3F nn = nor.normal();
m_a = nn.x;
m_b = nn.y;
m_c = nn.z;
m_d = - pop.dot(nn);
}
void Plane::create(const Vector3F & p0, const Vector3F & p1, const Vector3F & p2, const Vector3F & p3)
{
Vector3F cen = p0 * 0.25f + p1 * 0.25f + p2 * 0.25f + p3 * 0.25f;
Vector3F c0 = p2 - p0;
Vector3F c1 = p3 - p1;
Vector3F nn = c0.cross(c1);
nn.normalize();
m_a = nn.x;
m_b = nn.y;
m_c = nn.z;
m_d = - cen.dot(nn);
}
char BaseMesh::triangleIntersect(const Vector3F * threeCorners, IntersectionContext * ctx) const
{
Vector3F a = threeCorners[0];
Vector3F b = threeCorners[1];
Vector3F c = threeCorners[2];
Vector3F ab = b - a;
Vector3F ac = c - a;
Vector3F nor = ab.cross(ac);
nor.normalize();
Ray &ray = ctx->m_ray;
float ddotn = ray.m_dir.dot(nor);
if(!ctx->twoSided && ddotn > 0.f) return 0;
float t = (a.dot(nor) - ray.m_origin.dot(nor)) / ddotn;
if(t < 0.f || t > ray.m_tmax) return 0;
//printf("face %i %f %f", idx, t, ctx->m_minHitDistance);
if(t > ctx->m_minHitDistance) return 0;
Vector3F onplane = ray.m_origin + ray.m_dir * t;
Vector3F e01 = b - a;
Vector3F x0 = onplane - a;
if(e01.cross(x0).dot(nor) < 0.f) return 0;
//printf("pass a\n");
Vector3F e12 = c - b;
Vector3F x1 = onplane - b;
if(e12.cross(x1).dot(nor) < 0.f) return 0;
//printf("pass b\n");
Vector3F e20 = a - c;
Vector3F x2 = onplane - c;
if(e20.cross(x2).dot(nor) < 0.f) return 0;
//printf("pass c\n");
ctx->m_hitP = onplane;
ctx->m_hitN = nor;
ctx->m_minHitDistance = t;
ctx->m_geometry = (Geometry*)this;
ctx->m_success = 1;
return 1;
}
bool Patch::isBehind(const Vector3F & po, Vector3F & nr) const
{
int i;
float maxFacing = -1.f;
float facing;
Vector3F dv;
for(i = 0; i < 4; i++) {
dv = vertex(i) - po;
dv.normalize();
facing = nr.dot(dv);
if(facing > maxFacing) {
maxFacing = facing;
}
}
return maxFacing < 0.f;
}
bool Patch::pushPlane(PushPlaneContext * ctx) const
{
if(!ctx->m_componentBBox.isPointAround(ctx->m_origin, ctx->m_maxRadius)) return false;
ctx->m_currentAngle = 0.f;
if(ctx->m_componentBBox.isPointInside(ctx->m_origin)) return true;
Vector3F selfN;
getNormal(selfN);
if(selfN.dot(ctx->m_front) > 0.f) return false;
if(isBehind(ctx->m_origin, ctx->m_front)) return false;
if(isBehind(ctx->m_origin, ctx->m_up)) return false;
float ang;
Vector3F v, dv, dp, pop;
bool allBellow = true;
for(int i = 0; i < 4; i++) {
v = vertex(i);
dv = v - ctx->m_origin;
ctx->m_plane.projectPoint(v, pop);
dp = pop - ctx->m_origin;
dp.normalize();
ang = dp.angleBetween(dv, ctx->m_up);
if(ang > ctx->m_maxAngle) allBellow = false;
if(ang > ctx->m_currentAngle)
ctx->m_currentAngle = ang;
}
if(allBellow) return false;
return true;
}
void getRotationMatrix(const Vector3F &z_ICS, const Vector3F &n_ICS, Matrix3F & ICS_R_patch)
{
// z_ICS and n_ICS has to be normalized already !
Float cosTheta = z_ICS.dot(n_ICS);
Float Theta = acos(cosTheta);
Float sinTheta = sin(Theta);
if(Theta > 0)
{
Vector3F rotAxis_ICS;
rotAxis_ICS<< -n_ICS(1)/sinTheta, n_ICS(0)/sinTheta, 0;
rotAxis_ICS.normalize();
ICS_R_patch = Matrix3F::Identity()+ sinTheta*cr3(rotAxis_ICS) + (1-cosTheta)*cr3(rotAxis_ICS)*cr3(rotAxis_ICS);
}
else
{
ICS_R_patch = Matrix3F::Identity();
}
}
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
}
void GjkContactSolver::rayCast(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
separateDistance(A, B, result);
if(result->hasResult) return;
resetSimplex(result->W);
const Vector3F r = result->rayDirection;
float lamda = 0.f;
// ray started at origin
const Vector3F startP = Vector3F::Zero;
Vector3F hitP = startP;
Vector3F hitN; hitN.setZero();
Vector3F v = hitP - result->closestPoint;
Vector3F w, p, pa, pb, localA, localB;
float vdotw, vdotr;
int k = 0;
for(; k < 32; 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;
w = hitP - p;
vdotw = v.dot(w);
if(vdotw > 0.f) {
// std::cout<<" v.w > 0\n";
if(vdotr >= 0.f) {
// std::cout<<" v.r >= 0 missed\n";
result->hasResult = 0;
return;
}
lamda -= vdotw / vdotr;
hitP = startP + r * lamda;
hitN = v;
}
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;
smallestSimplex(result);
}
if(k==32) std::cout<<" max iterations reached!\n";
// std::cout<<" k"<<k<<" ||v|| "<<v.length()<<"\n";
result->hasResult = 1;
result->separateAxis = hitN.normal();
result->distance = lamda;
}
float FEMTetrahedronMesh::getTetraVolume(Vector3F e1, Vector3F e2, Vector3F e3) {
return e1.dot( e2.cross( e3 ) )/ 6.0f;
}
void GeometryCorrectionTable::apply_correction_to_out_plane_edge(
const Vector3F& edge_dir, MatrixFr& loop) {
const Float EPS = 1e-3;
assert(fabs(edge_dir[2]) > 0.0);
VectorF bbox_min = loop.colwise().minCoeff();
VectorF bbox_max = loop.colwise().maxCoeff();
VectorF bbox_center = 0.5 * (bbox_min + bbox_max);
size_t num_vts = loop.rows();
size_t dim = loop.cols();
assert(dim == 3);
MatrixFr proj_loop(num_vts, 3);
Vector3F proj_edge_dir(edge_dir[0], edge_dir[1], 0.0);
if (loop.rows() != 4) {
throw NotImplementedError(
"Geometry correction supports only square wires");
}
for (size_t i=0; i<num_vts; i++) {
VectorF v = loop.row(i) - bbox_center.transpose();
Float edge_dir_offset = v[2] / edge_dir[2];
VectorF proj_v = v - edge_dir * edge_dir_offset;
proj_loop.row(i) = proj_v;
assert(fabs(proj_v[2]) < EPS);
}
Float dist_01 = (proj_loop.row(0) - proj_loop.row(1)).norm();
Float dist_12 = (proj_loop.row(1) - proj_loop.row(2)).norm();
if (dist_01 > dist_12) {
proj_edge_dir = (proj_loop.row(0) - proj_loop.row(1)) / dist_01;
} else {
proj_edge_dir = (proj_loop.row(1) - proj_loop.row(2)) / dist_12;
}
const VectorF& corner = proj_loop.row(0);
Float target_half_height = proj_edge_dir.dot(corner);
Float target_half_width =
(corner - proj_edge_dir * target_half_height).norm();
target_half_height = fabs(target_half_height);
Vector2F correction_1 = lookup(target_half_width, target_half_height);
Vector2F correction_2 = lookup(target_half_height, target_half_width);
Float half_width = 0.5 * (correction_1[0] + correction_2[1])
+ 0.05 * num_offset_pixel;
Float half_height = 0.5 * (correction_1[1] + correction_2[0])
+ 0.05 * num_offset_pixel;
half_width = std::max(half_width, min_thickness);
half_height = std::max(half_height, min_thickness);
for (size_t i=0; i<num_vts; i++) {
const VectorF& proj_v = proj_loop.row(i);
Float height = proj_edge_dir.dot(proj_v);
VectorF width_dir = (proj_v - proj_edge_dir * height).normalized();
assert(!isnan(width_dir[0]));
assert(!isnan(width_dir[1]));
assert(!isnan(width_dir[2]));
Float height_sign = (height < 0.0)? -1: 1;
proj_loop.row(i) = proj_edge_dir * height_sign * half_height
+ width_dir * half_width;
}
for (size_t i=0; i<num_vts; i++) {
const VectorF& proj_v = proj_loop.row(i);
loop.row(i) = (bbox_center + proj_v - edge_dir *
edge_dir.dot(proj_v)).transpose();
}
}
float Vector3F::angleBetween(const Vector3F& another, const Vector3F& up) const
{
float ang = acos(this->normal().dot(another.normal()));
if(another.dot(up) >= 0.f) return ang;
return -ang;
}
char Facet::isVertexAbove(const Vertex & v) const
{
Vector3F dv = *v.m_v - getCentroid();
dv.normalize();
return dv.dot(m_normal) > 0.0f;
}
void BCIViz::findNeighbours()
{
if(!checkHull()) return;
const int numTri = m_hull->getNumFace();
Vector3F d;
d.x = fDriverPos.x;
d.y = fDriverPos.y;
d.z = fDriverPos.z;
d.normalize();
m_hitTriangle = 0;
for(int i = 0; i < numTri; i++)
{
Facet f = m_hull->getFacet(i);
Vertex p0 = f.getVertex(0);
const Vector3F nor = f.getNormal();
float ddotn = d.dot(nor);
if(ddotn < 10e-5 && ddotn > -10e-5) continue;
float t = p0.dot(nor) / ddotn;
if(t < 0.f) continue;
Vertex p1 = f.getVertex(1);
Vertex p2 = f.getVertex(2);
m_hitTriangle = i;
m_hitP = d * t;
Vector3F e01 = p1 - p0;
Vector3F e02 = p2 - p0;
Vector3F tmp = e01.cross(e02);
if(tmp.dot(nor) < 0.f) {
Vertex sw = p1;
p1 = p2;
p2 = sw;
}
e01 = p1 - p0;
Vector3F x0 = m_hitP - p0;
Vector3F e12 = p2 - p1;
Vector3F x1 = m_hitP - p1;
Vector3F e20 = p0 - p2;
Vector3F x2 = m_hitP - p2;
neighbourId[0] = p0.getIndex();
neighbourId[1] = p1.getIndex();
neighbourId[2] = p2.getIndex();
if(e01.cross(x0).dot(nor) < 0.f) continue;
if(e12.cross(x1).dot(nor) < 0.f) continue;
if(e20.cross(x2).dot(nor) < 0.f) continue;
return;
}
}
void drawArrow2(Vector3F & location, Vector3F & direction, double headLength)
{
Vector3F zup;
zup << 0,0,1;
Vector3F normedDirection = direction.normalized();
// Note: We need to create a rotation such that the z-axis points in the direction of the 'direction' argument
// Thus, we can realize the rotation with a pure x-y axial rotation
// The rotation matrix of the rotated frame 'r' to the current frame 'c' (c_R_r) has special form.
// It's 3-rd column in particular has form: [ wy*sin(theta) -wx*sin(theta) (1- cos(theta))]'
// where theta , [wx wy 0] is the angle, axis of rotation
// Find the rotation angle by comparing the z-axis of the current and rotated frame
const double cosTheta = zup.dot(normedDirection);
const double theta = acos(cosTheta);
const double sinTheta = sin(theta);
// Exploit the special form of the rotation matrix (explained above) for find the axis of rotation
double rX, rY, rZ;
if(theta > 0) {
rX = - normedDirection(1)/sinTheta;
rY = normedDirection(0)/sinTheta;
rZ = 0;
}
else {
rX = 0;
rY = 0;
rZ = 1;
}
// before calling this function, be sure glMatrixMode is GL_MODELVIEW and camera->applyView is called.
glPushMatrix();
glTranslatef(location(0), location(1), location(2));
glRotated(RADTODEG * theta, rX, rY, rZ);
double detail = 16;
double headWidth = 0.01;
double shaftLength = direction.norm();
if (shaftLength > headLength)
{
shaftLength -= headLength;
}
else
{
headLength = shaftLength ;
shaftLength = 0;
}
glBegin(GL_LINES);
glVertex3f(0.0, 0.0,shaftLength );
glVertex3f(0.0, 0.0, 0.0);
glEnd();
glTranslatef(0, 0, shaftLength);
//Draw Arrowhead
gluCylinder(frame->glPane->quadratic,headWidth,0.0f,headLength,detail,detail);
//Draw Arrowhead Base
gluDisk(frame->glPane->quadratic,0.0f,headWidth,detail,detail);
glPopMatrix();
}