C++ mat3类代码示例

mat3 operator +(const mat3 &_lhs, const mat3 &_rhs)
{
    mat3 m;
    for(int i = 0; i < 9; ++i)
        m.set( i, _lhs.get(i) + _rhs.get(i) );
    return m;
}

void Leaf::fitBox(const mat3 &R, vec3 &center, vec3 &halfSize)
{
	vec3 x = R.row(0);
	vec3 y = R.row(1);
	vec3 z = R.row(2);
	vec3 max(-1.0e10, -1.0e10, -1.0e10);
	vec3 min( 1.0e10,  1.0e10,  1.0e10);
	std::list<Triangle>::iterator it;
	for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
		boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);
		boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);
		boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);
	}
	DBGP("Max: " << max);
	DBGP("Min: " << min);
	for (int i=0; i<3; i++) {
		halfSize[i] = 0.5 * (max[i] - min[i]);
	}
	DBGP("computed halfsize: " << halfSize);
	//halfSize = 0.5 * (max - min);
	center = min + halfSize;
	center = R.inverse() * center;
	//sanity check
	for (int i=0; i<3; i++) {
		if (halfSize[i] < TOLERANCE) {
			if (halfSize[i] < 0.5 * TOLERANCE) {
				DBGA("Warning: degenerate box computed");
			}
			halfSize[i] = TOLERANCE;
		}
	}
	DBGP("returned halfsize: " << halfSize);
}

vec3 et::removeMatrixScale(mat3& mat)
{
	vec3 c0 = mat.column(0);
	vec3 c1 = mat.column(1);
	vec3 c2 = mat.column(2);

	float lengths[3] = { c0.length(), c1.length(), c2.length() };
	
	ET_ASSERT(lengths[0] > 0.0f);
	ET_ASSERT(lengths[1] > 0.0f);
	ET_ASSERT(lengths[2] > 0.0f);

	if (mat.determinant() < 0.0f)
	{
		float minValues[3] =
		{
			etMin(c0.x, etMin(c0.y, c0.z)),
			etMin(c1.x, etMin(c1.y, c1.z)),
			etMin(c2.x, etMin(c2.y, c2.z))
		};
		auto offset = std::min_element(minValues, minValues + 3) - minValues;
		lengths[offset] = -lengths[offset];
	}

	for (size_t i = 0; i < 3; ++i)
	{
		mat[0][i] /= lengths[i];
		mat[1][i] /= lengths[i];
		mat[2][i] /= lengths[i];
	}
	
	return vec3(lengths[0], lengths[1], lengths[2]);
}

/*!
  Converts this quaternion to a 3x3 rotation matrix.
*/
void
Quaternion::ToRotationMatrix(mat3 &R) const
{
    double tx  = 2.0*x;
    double ty  = 2.0*y;
    double tz  = 2.0*z;
    double twx = tx*w;
    double twy = ty*w;
    double twz = tz*w;
    double txx = tx*x;
    double txy = ty*x;
    double txz = tz*x;
    double tyy = ty*y;
    double tyz = tz*y;
    double tzz = tz*z;

    R.element(0,0) = 1.0-(tyy+tzz);
    R.element(1,0) = txy-twz;
    R.element(2,0) = txz+twy;
    R.element(0,1) = txy+twz;
    R.element(1,1) = 1.0-(txx+tzz);
    R.element(2,1) = tyz-twx;
    R.element(0,2) = txz-twy;
    R.element(1,2) = tyz+twx;
    R.element(2,2) = 1.0-(txx+tyy);
}

void
PropertyManager::updateMat3(wxPropertyGridManager *pg, std::string label, mat3 a) {

	std::string s = label + ".Row0.x";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(0,0));
	s.clear();
	s = label + ".Row0.y";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(0, 1));
	s.clear();
	s = label + ".Row0.z";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(0, 2));

	s.clear();
	s = label + ".Row1.x";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(1, 0));
	s.clear();
	s = label + ".Row1.y";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(1, 1));
	s.clear();
	s = label + ".Row1.z";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(1, 2));

	s.clear();
	s = label + ".Row2.x";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(2, 0));
	s.clear();
	s = label + ".Row2.y";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(2, 1));
	s.clear();
	s = label + ".Row2.z";
	pg->SetPropertyValue(wxString(s.c_str()), a.at(2, 2));
}

/* order.                                                              */
vec3 operator* (const vec3& v, const mat3& m)
{
	/* Multiply column one by the vector to get the new x component */
	float x = m.getColumn(0) * v;
	/* Multiply column two by the vector to get the new y component */
	float y = m.getColumn(1) * v;
	/* Multiply column three by the vector to get the new z component */
	float z = m.getColumn(2) * v;

	/* Return a new vector with the new components */
	return vec3(x, y, z);
}

void Graphics::setMatrix(ConstantLocation location, const mat3& value) {
	FRHICommandListImmediate& commandList = GRHICommandList.GetImmediateCommandList();
	TShaderMapRef<FVertexShaderExample> VertexShader(GetGlobalShaderMap(ERHIFeatureLevel::SM5));
	mat3 value2 = value.Transpose();
	float floats[12];
	for (int y = 0; y < 3; ++y) {
		for (int x = 0; x < 3; ++x) {
			floats[y * 4 + x] = value.get(y, x);
		}
	}
	commandList.SetShaderParameter(VertexShader->GetVertexShader(), location.parameter.GetBufferIndex(), location.parameter.GetBaseIndex(), 4 * 12, floats);
}

void ComponentPhysicsGeom::setOrientation(const mat3 &m)
{
	dMatrix3 r;

	const vec3 x = m.getAxisX().getNormal();
	const vec3 y = m.getAxisY().getNormal();
	const vec3 z = m.getAxisZ().getNormal();

	r[0] = x.x; r[1] = x.y; r[2] = x.z; r[3] = 0.0f;
	r[4] = y.x; r[5] = y.y; r[6] = y.z; r[7] = 0.0f;
	r[8] = z.x; r[9] = z.y; r[10]= z.z; r[11]= 0.0f;

	dGeomSetRotation(geom, r);

	getParentBlackBoard().relayMessage(MessageOrientationHasBeenSet(getOrientation()));
}

inline mat3 inverse(mat3 const& m)
{
	scalar_t const d = 1 / m.determinant();
	if (d != 0)
	{
		scalar_t const id = 1 / d;
		return mat3(
			vec3(
				 id * (m.y.y * m.z.z - m.y.z * m.z.y),
				-id * (m.x.y * m.z.z - m.x.z * m.z.y),
				 id * (m.x.y * m.y.z - m.x.z * m.y.y)),

			 vec3(
				-id * (m.y.x * m.z.z - m.y.z * m.z.x),
				 id * (m.x.x * m.z.z - m.x.z * m.z.x),
				-id * (m.x.x * m.y.z - m.x.z * m.y.x)),

			vec3(
				 id * (m.y.x * m.z.y - m.y.y * m.z.x),
				-id * (m.x.x * m.z.y - m.x.y * m.z.x),
				 id * (m.x.x * m.y.y - m.x.y * m.y.x)));
	}
	else
	{
		return mat3::identity();
	}
}

float determinant(const mat3 &_m)
{
    float a = _m.get(0,0);
    float b = _m.get(1,0);
    float c = _m.get(2,0);
    float d = _m.get(0,1);
    float e = _m.get(1,1);
    float f = _m.get(2,1);
    float g = _m.get(0,2);
    float h = _m.get(1,2);
    float i = _m.get(2,2);

    return a * (e*i - f*h) -
            b * (d*i - f*g) +
            c * (d*h - e*g);
}

mat3 transpose(const mat3 &_m)
{
    mat3 r;
    for(int i = 0; i < 3; ++i)
        for(int j = 0; j < 3; ++j)
            r.set(j, i, _m.get(i, j));
		return r;
}

void ComponentPhysicsGeom::drawAxes() const
{
	CHECK_GL_ERROR();
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	{
		glLineWidth(2.0f);

		const mat3 orientation = getOrientation();
		mat4 transformation(getPosition(),
			                orientation.getAxisX(),
							orientation.getAxisY(),
							orientation.getAxisZ());

		glPushMatrix();
		glMultMatrixf(transformation);

		glBegin(GL_LINES);
		glColor4fv(red);
		glVertex3fv(vec3(0,0,0));
		glVertex3fv(orientation.getAxisX());

		glColor4fv(green);
		glVertex3fv(vec3(0,0,0));
		glVertex3fv(orientation.getAxisY());

		glColor4fv(blue);
		glVertex3fv(vec3(0,0,0));
		glVertex3fv(orientation.getAxisZ());
		glEnd();

		glPopMatrix();
	}
	glPopAttrib();
	CHECK_GL_ERROR();
}

// Apply Householder reflection represented by u to column vectors of M
void reflect_cols(mat3 &M, vec3 &u)
{
    for (int i=0; i < 3; ++i) 
    {
        nv_scalar s = dot(u , M.col(i));
        for (int j=0; j < 3; ++j) 
            M(j,i) -= u[j]*s;
    }
}

bool mat3::operator ==(const mat3& rhs)
{
	for(int i = 0; i < VEC_DIM; i++)
	{
		if(v[i] != rhs.getCol(i))
			return false;
	}
	return true;
}

void Graphics::setMatrix(ConstantLocation location, const mat3& value) {
	if (location.shaderType == -1) return;
	float floats[12];
	for (int y = 0; y < 3; ++y) {
		for (int x = 0; x < 3; ++x) {
			floats[y * 4 + x] = value.get(y, x);
		}
	}
	if (location.shaderType == 0) device->SetVertexShaderConstantF(location.reg.regindex, floats, 3);
	else device->SetPixelShaderConstantF(location.reg.regindex, floats, 3);
}