C++ Vector3r::transpose方法代码示例

// http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
Matrix3r woo::Volumetric::triangleInertia(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2){
	Matrix3r V; V<<v0.transpose(),v1.transpose(),v2.transpose(); // rows!
	Real a=((v1-v0).cross(v2-v0)).norm(); // twice the triangle area
	Matrix3r S; S<<2,1,1, 1,2,1, 1,1,2; S*=(1/24.);
	Matrix3r C=a*V.transpose()*S*V;
	return Matrix3r::Identity()*C.trace()-C;
};

// grid sampling
Matrix3r woo::Volumetric::tetraInertia_grid(const Vector3r v[4], int div){
	AlignedBox3r b; for(int i:{0,1,2,3}) b.extend(v[i]);
	std::cerr<<"bbox "<<b.min()<<", "<<b.max()<<std::endl;
	Real dd=b.sizes().minCoeff()/div;
	Vector3r xyz;
	// point inside test: http://steve.hollasch.net/cgindex/geometry/ptintet.html
	typedef Eigen::Matrix<Real,4,4> Matrix4r;
	Matrix4r M0; M0<<v[0].transpose(),1,v[1].transpose(),1,v[2].transpose(),1,v[3].transpose(),1;
	Real D0=M0.determinant();
	// Matrix3r I(Matrix3r::Zero());
	Matrix3r C(Matrix3r::Zero());
	Real dV=pow(dd,3);
	// std::ofstream dbg("/tmp/tetra.txt");
	for(xyz.x()=b.min().x()+dd/2.; xyz.x()<b.max().x(); xyz.x()+=dd){
		for(xyz.y()=b.min().y()+dd/2.; xyz.y()<b.max().y(); xyz.y()+=dd){
			for(xyz.z()=b.min().z()+dd/2.; xyz.z()<b.max().z(); xyz.z()+=dd){
				bool inside=true;
				for(int i:{0,1,2,3}){
					Matrix4r D=M0;
					D.row(i).head<3>()=xyz;
					if(std::signbit(D.determinant())!=std::signbit(D0)){ inside=false; break; }
				}
				if(inside){
					C+=dV*(xyz*xyz.transpose());
					// dbg<<xyz[0]<<" "<<xyz[1]<<" "<<xyz[2]<<" "<<dd/2.<<endl;
				}
			}
		}
	}
	return Matrix3r::Identity()*C.trace()-C;
}

		Matrix3r inertiaTranslate(const Matrix3r& I, const Real m, const Vector3r& off){
			return I+m*(off.dot(off)*Matrix3r::Identity()-off*off.transpose());
		}

void SphereClumpGeom::recompute(int _div, bool failOk, bool fastOnly){
	if((centers.empty() && radii.empty()) || centers.size()!=radii.size()){
		if(failOk) { makeInvalid(); return;}
		throw std::runtime_error("SphereClumpGeom.recompute: centers and radii must have the same length (len(centers)="+to_string(centers.size())+", len(radii)="+to_string(radii.size())+"), and may not be empty.");
	}
	div=_div;
	// one single sphere: simple
	if(centers.size()==1){
		pos=centers[0];
		ori=Quaternionr::Identity();
		volume=(4/3.)*M_PI*pow(radii[0],3);
		inertia=Vector3r::Constant((2/5.)*volume*pow(radii[0],2));
		equivRad=radii[0];
		return;
	}
	volume=0;
	Vector3r Sg=Vector3r::Zero();
	Matrix3r Ig=Matrix3r::Zero();
	if(_div<=0){
		// non-intersecting: Steiner's theorem
		for(size_t i=0; i<centers.size(); i++){
			const Real& r(radii[i]); const Vector3r& x(centers[i]);
			Real v=(4/3.)*M_PI*pow(r,3);
			volume+=v;
			Sg+=v*x;
			Ig+=woo::Volumetric::inertiaTensorTranslate(Vector3r::Constant((2/5.)*v*pow(r,2)).asDiagonal(),v,-1.*x);
		}
	} else {
		// intersecting: grid sampling
		Real rMin=Inf; AlignedBox3r aabb;
		for(size_t i=0; i<centers.size(); i++){
			aabb.extend(centers[i]+Vector3r::Constant(radii[i]));
			aabb.extend(centers[i]-Vector3r::Constant(radii[i]));
			rMin=min(rMin,radii[i]);
		}
		if(rMin<=0){
			if(failOk){ makeInvalid(); return; }
			throw std::runtime_error("SphereClumpGeom.recompute: minimum radius must be positive (not "+to_string(rMin)+")");
		}
		Real dx=rMin/_div; Real dv=pow(dx,3);
		long nCellsApprox=(aabb.sizes()/dx).prod();
		 // don't compute anything, it would take too long
		if(fastOnly && nCellsApprox>1e5){ makeInvalid(); return; }
		if(nCellsApprox>1e8) LOG_WARN("SphereClumpGeom: space grid has "<<nCellsApprox<<" cells, computing inertia can take a long time.");
		Vector3r x;
		for(x.x()=aabb.min().x()+dx/2.; x.x()<aabb.max().x(); x.x()+=dx){
			for(x.y()=aabb.min().y()+dx/2.; x.y()<aabb.max().y(); x.y()+=dx){
				for(x.z()=aabb.min().z()+dx/2.; x.z()<aabb.max().z(); x.z()+=dx){
					for(size_t i=0; i<centers.size(); i++){
						if((x-centers[i]).squaredNorm()<pow(radii[i],2)){
							volume+=dv;
							Sg+=dv*x;
							Ig+=dv*(x.dot(x)*Matrix3r::Identity()-x*x.transpose())+/*along princial axes of dv; perhaps negligible?*/Matrix3r(Vector3r::Constant(dv*pow(dx,2)/6.).asDiagonal());
							break;
						}
					}
				}
			}
		}
	}
	woo::Volumetric::computePrincipalAxes(volume,Sg,Ig,pos,ori,inertia);
	equivRad=(inertia.array()/volume).sqrt().mean(); // mean of radii of gyration
}