예제 #1
0
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();
}
예제 #2
0
파일: Sculptor.cpp 프로젝트: spinos/aphid
/// 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);
}
예제 #3
0
파일: Plane.cpp 프로젝트: kkaushalp/aphid
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);
}
예제 #4
0
파일: Plane.cpp 프로젝트: kkaushalp/aphid
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);
}
예제 #5
0
파일: BaseMesh.cpp 프로젝트: spinos/aphid
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;
}
예제 #6
0
파일: Patch.cpp 프로젝트: spinos/aphid
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;
}
예제 #7
0
파일: Patch.cpp 프로젝트: spinos/aphid
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();
	}
}
예제 #9
0
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;
}
예제 #10
0
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
}
예제 #11
0
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;
}
예제 #12
0
float FEMTetrahedronMesh::getTetraVolume(Vector3F e1, Vector3F e2, Vector3F e3) {
	return  e1.dot( e2.cross( e3 ) )/ 6.0f;
}
예제 #13
0
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();
    }
}
예제 #14
0
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;
}
예제 #15
0
파일: Facet.cpp 프로젝트: ahmidou/aphid
char Facet::isVertexAbove(const Vertex & v) const
{
	Vector3F dv = *v.m_v - getCentroid();
	dv.normalize();
	return dv.dot(m_normal) > 0.0f;
}
예제 #16
0
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;
	}
}
예제 #17
0
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();
	
}