C++ Matrix44F类代码示例

void TransformManipulator::spin(const Vector3F & d)
{
	Matrix44F ps;
	parentSpace(ps);
	Matrix44F invps = ps;
	invps.inverse();
	
	const Vector3F worldP = ps.transform(translation());
	const Vector3F rotUp = ps.transformAsNormal(hitPlaneNormal());
	
	Vector3F toa = m_currentPoint - worldP;
	Vector3F tob = toa + d;
	
	toa.normalize();
	tob.normalize();
	float ang = toa.angleBetween(tob, toa.cross(rotUp).reversed());
	
	Vector3F angles;
	
	if(m_rotateAxis == AY) angles.set(0.f, ang, 0.f);
	else if(m_rotateAxis == AZ) angles.set(0.f, 0.f, ang);
	else angles.set(ang, 0.f, 0.f);
	
	m_subject->rotate(angles);
	setRotationAngles(m_subject->rotationAngles());
}

void TransformManipulator::perform(const Ray * r)
{
	if(isDetached()) return;
	const Vector3F worldP = worldSpace().getTranslation();
	Matrix44F ps;
	parentSpace(ps);
	Plane pl(ps.transformAsNormal(hitPlaneNormal()), worldP);
	Vector3F hit, d;
    float t;
	if(pl.rayIntersect(*r, hit, t, 1)) {
		d = hit - worldP;
		if(d.length() > 10.f) return;
		
		if(m_mode == ToolContext::RotateTransform) {
			d = d.normal() * 8.f;
			hit = worldP + d;
		}
		
		d = hit - m_currentPoint;
		
		if(m_mode == ToolContext::MoveTransform)
			move(d);
		else
			spin(d);
			
		m_currentPoint = hit;
	}
}

Matrix44F BaseTransform::space() const
{
	Matrix44F s;
	s.setTranslation(m_translation);
	s.setRotation(rotation());
	return s;
}

const Vector3F Chassis::torsionBarHinge(const int & i, bool isLeft) const
{
    const Matrix44F mat = bogieArmOrigin(i, isLeft);
    Vector3F p = mat.getTranslation();
    p.z += 0.5f * m_bogieArmLength * cos(m_torsionBarRestAngle);
    p.y += 0.5f * m_bogieArmLength * sin(m_torsionBarRestAngle);
	return p;
}

void Matrix44F::rotateY(float beta)
{
	const float c = cos(beta);
	const float s = sin(beta);
	Matrix44F r;
	*r.m(0, 0) = c; *r.m(0, 2) = -s;
	*r.m(2, 0) = s; *r.m(2, 2) = c;
	multiply(r);
}

Matrix44F BaseTransform::space() const
{
    Matrix44F s;
	s.setTranslation(m_translation);
	Matrix33F r = orientation();
	
	s.translate(m_rotatePivotTranslate);
	s.translate(m_rotatePivot);
	s.translate(r.transform(m_rotatePivot.reversed()));
	
	s.translate(r.transform(m_scalePivotTranslate));
	s.translate(r.transform(m_scalePivot));
	
	Vector3F displaceByScaling = m_scalePivot.reversed();
	displaceByScaling = displaceByScaling * m_scale;
	s.translate(r.transform(displaceByScaling));
	
	Matrix33F scaleMatrix;
	*scaleMatrix.m(0, 0) = m_scale.x;
	*scaleMatrix.m(1, 1) = m_scale.y;
	*scaleMatrix.m(2, 2) = m_scale.z;
	
	r = scaleMatrix * r;
	s.setRotation(r);

	return s;
}

const Matrix44F Chassis::computeBogieArmOrigin(const float & chassisWidth, const Vector3F & wheelP, const float & l, const float & s, const float & ang) const
{
    Matrix44F tm;
    tm.rotateX(-ang);
    Vector3F p;
    p.x = chassisWidth * .5f + s * .5f;
    p.y = wheelP.y + l * .5f * sin(ang);
    p.z = wheelP.z + l * .5f * cos(ang);
    tm.setTranslation(p);
    return tm;
}

const Matrix44F Matrix44F::transformBy(const Matrix44F & a) const
{
	Matrix44F t; 
	int i, j;
	for(i = 0; i < 4; i++) {
		for(j = 0; j < 4; j++) {
			*t.m(i, j) = M(i, 0) * a.M(0, j) + M(i, 1) * a.M(1, j) + M(i, 2) * a.M(2, j) + M(i, 3) * a.M(3, j);
		}
	}

	return t;
}

const Matrix44F  Chassis::bogieArmOrigin(const int & i, bool isLeft) const
{
    Matrix44F res;
    res.rotateX(-m_torsionBarRestAngle);
    Vector3F cen = roadWheelOrigin(i, isLeft);
    float d = 1.f;
	if(!isLeft) d = -d;
	cen.x += -m_trackWidth * .5f * d + m_bogieArmWidth * .7f * d;
    cen.z += 0.5f * m_bogieArmLength * cos(m_torsionBarRestAngle);
    cen.y += 0.5f * m_bogieArmLength * sin(m_torsionBarRestAngle);
    res.setTranslation(cen);
    return res;
}

Vector3F SkeletonSubspaceDeformer::combine(unsigned idx)
{
    const unsigned nj = numBindJoints(idx);
    unsigned j;
    Vector3F q;
    Matrix44F space;
    for(j = 0; j < nj; j++) {
        space = bindS(idx, j);
        q += space.transform(bindP(idx, j)) * bindW(idx, j);
    }
    
    return q;
}

bool BaseTransform::intersect(const Ray & ray) const
{	
	Matrix44F s = worldSpace();
	s.inverse();
	Vector3F a = ray.m_origin + ray.m_dir * ray.m_tmin;
	a = s.transform(a);
	Vector3F b = ray.m_origin + ray.m_dir * ray.m_tmax;
	b = s.transform(b);
	Ray objR(a, b);
	
	float hit0, hit1;
	return getBBox().intersect(objR, &hit0, &hit1);
}

void BaseView::frameAll(const BoundingBox & b)
{
	Vector3F eye = b.center();
	eye.z = b.getMax(2) + b.distance(0) / hfov() * .55f + 120.f;
	setEyePosition(eye);
	
	Matrix44F m;
	m.setTranslation(eye);
	*cameraSpaceR() = m;
	m.inverse();
	*cameraInvSpaceR() = m;
	setFrustum(1.33f, 1.f, 26.2f, -1.f, -1000.f);
}

void BaseTransform::parentSpace(Matrix44F & dst) const
{
	if(!parent()) return;
	
	BaseTransform * p = parent();
	while(p) {
		dst.multiply(p->space());
		p = p->parent();
	}
}

void SkeletonSubspaceDeformer::calculateSubspaceP()
{
	unsigned i, j, n, nj, vstart;
	Matrix44F spaceInv;
	Vector3F p;
	for(i = 0; i < numVertices(); i++) {
		p = getDeformedP()[i];
		n = m_jointIds[i]._ndim;
		nj = n - 1;
		vstart = m_jointIds[i][nj];
		
		for(j = 0; j < nj; j++) {
			SkeletonJoint * joint = m_skeleton->jointByIndex(m_jointIds[i][j]);
			spaceInv = joint->worldSpace();
			spaceInv.inverse();
			m_subspaceP[vstart + j] = spaceInv.transform(p);
		}
	}
}

void MlUVView::drawControlVectors(MlFeather * f)
{
	Vector3F baseP(f->baseUV());
	GeoDrawer * dr = getDrawer();
	
	glPushMatrix();
    Matrix44F s;
	s.setTranslation(baseP);
	
	float * quill = f->getQuilly();
	Vector3F b, a;
	Vector2F *d;
	for(short i=0; i <= f->numSegment(); i++) {
	    dr->useSpace(s);
		
		a.setZero();
		d = f->uvDisplaceAt(i, 0);
		for(short j = 0; j < 3; j++) {
			b = d[j];
			dr->arrow(a, a + b);
			a += b;
		}
		
		a.setZero();
		d = f->uvDisplaceAt(i, 1);
		for(short j = 0; j < 3; j++) {
			b = d[j];
			dr->arrow(a, a + b);
			a += b;
		}
		
		a.setZero();
		b.set(0.f, quill[i], 0.f);
		
		if(i<f->numSegment()) dr->arrow(a, b);
		
		s.setTranslation(b);
	}
	glPopMatrix();
}