WTransform WTransform::adjoint() const
{
return WTransform(m33() * m22() - m32() * m23(),
- (m33() * m12() - m32() * m13()),
- (m33() * m21() - m31() * m23()),
m33() * m11() - m31() * m13(),
m32() * m21() - m31() * m22(),
- (m32() * m11() - m31() * m12()));
}
FloatPoint TransformationMatrix::projectPoint(const FloatPoint& p) const
{
// This is basically raytracing. We have a point in the destination
// plane with z=0, and we cast a ray parallel to the z-axis from that
// point to find the z-position at which it intersects the z=0 plane
// with the transform applied. Once we have that point we apply the
// inverse transform to find the corresponding point in the source
// space.
//
// Given a plane with normal Pn, and a ray starting at point R0 and
// with direction defined by the vector Rd, we can find the
// intersection point as a distance d from R0 in units of Rd by:
//
// d = -dot (Pn', R0) / dot (Pn', Rd)
double x = p.x();
double y = p.y();
double z = -(m13() * x + m23() * y + m43()) / m33();
double outX = x * m11() + y * m21() + z * m31() + m41();
double outY = x * m12() + y * m22() + z * m32() + m42();
double w = x * m14() + y * m24() + z * m34() + m44();
if (w != 1 && w != 0) {
outX /= w;
outY /= w;
}
return FloatPoint(static_cast<float>(outX), static_cast<float>(outY));
}
WTransform WTransform::adjoint() const
{
WTransform res = WTransform(m33() * m22() - m32() * m23(),
- (m33() * m12() - m32() * m13()),
- (m33() * m21() - m31() * m23()),
m33() * m11() - m31() * m13(),
m32() * m21() - m31() * m22(),
- (m32() * m11() - m31() * m12()));
if (isJavaScriptBound()) {
res.assignBinding(*this,
WT_CLASS ".gfxUtils.transform_adjoint(" + jsRef() + ")");
}
return res;
}
void gnuext(void) {
#define m11(x, y...) stringify(x + y)
expect_string("2 + 18", m11(2, 18));
expect_string("2 +", m11(2));
#define m12(x, y...) stringify((x, ## y))
expect_string("(1)", m12(1));
expect_string("(1, 2)", m12(1, 2));
#define m13(x, y) stringify((x, ## y))
expect_string("(1,)", m13(1,));
expect_string("(1,2)", m13(1, 2));
}
const String DOMMatrixReadOnly::toString() const {
std::stringstream stream;
if (is2D()) {
stream << "matrix(" << a() << ", " << b() << ", " << c() << ", " << d()
<< ", " << e() << ", " << f();
} else {
stream << "matrix3d(" << m11() << ", " << m12() << ", " << m13() << ", "
<< m14() << ", " << m21() << ", " << m22() << ", " << m23() << ", "
<< m24() << ", " << m31() << ", " << m32() << ", " << m33() << ", "
<< m34() << ", " << m41() << ", " << m42() << ", " << m43() << ", "
<< m44();
}
stream << ")";
return String(stream.str().c_str());
}
// ONLY USE WHEN MASS MATRIX CONTAINS FREE BASE TRANSLATION AND ROTATION
// Since the translation and rotation are switched
// WBI order [T R Q]
// ocra order [R T Q]
/* static */ bool ocraWbiConversions::wbiToOcraMassMatrix(int qdof, const Eigen::MatrixXd &M_wbi, Eigen::MatrixXd &M_ocra)
{
int dof = qdof + DIM_T + DIM_R;
if(dof != M_wbi.cols() || dof != M_wbi.rows() || dof != M_ocra.rows() || dof != M_ocra.cols())
{
std::cout<<"ERROR: Input and output matrices - Is the model free root?" <<std::endl;
return false;
}
// ORIGINAL MATRIX BLOCKS
Eigen::MatrixXd m11(DIM_T, DIM_T);
m11 = M_wbi.block(0, 0, DIM_T, DIM_T);
Eigen::MatrixXd m12(DIM_T, DIM_R);
m12 = M_wbi.block(0, DIM_T, DIM_T, DIM_R);
Eigen::MatrixXd m13(DIM_T, qdof);
m13 = M_wbi.block(0, DIM_T+DIM_R, DIM_T, qdof);
Eigen::MatrixXd m21(DIM_R, DIM_T);
m21 = M_wbi.block(DIM_T, 0, DIM_R, DIM_T);
Eigen::MatrixXd m22(DIM_R, DIM_R);
m22 = M_wbi.block(DIM_T, DIM_T, DIM_R, DIM_R);
Eigen::MatrixXd m23(DIM_R, qdof);
m23 = M_wbi.block(DIM_T, DIM_T+DIM_R, DIM_R, qdof);
Eigen::MatrixXd m31(qdof, DIM_T);
m31 = M_wbi.block(DIM_T+DIM_R, 0, qdof, DIM_T);
Eigen::MatrixXd m32(qdof, DIM_R);
m32 = M_wbi.block(DIM_T+DIM_R, DIM_T, qdof, DIM_R);
Eigen::MatrixXd m33(qdof, qdof);
m33 = M_wbi.block(DIM_T+DIM_R, DIM_T+DIM_R, qdof, qdof);
M_ocra << m22, m21, m23,
m12, m11, m13,
m32, m31, m33;
return true;
}
bool
CMathGeom3D::
FourPointSphere(double x1, double y1, double z1, double x2, double y2, double z2,
double x3, double y3, double z3, double x4, double y4, double z4,
double *xc, double *yc, double *zc, double *r)
{
double d1 = x1*x1 + y1*y1 + z1*z1;
double d2 = x2*x2 + y2*y2 + z2*z2;
double d3 = x3*x3 + y3*y3 + z3*z3;
double d4 = x4*x4 + y4*y4 + z4*z4;
CMatrix3DH m11(x1, y1, z1, 1.0, x2, y2, z2, 1.0, x3, y3, z3, 1.0, x4, y4, z4, 1.0);
double dm11 = m11.determinant();
if (fabs(dm11) < CMathGen::EPSILON_E6)
return false;
double idm11 = 1.0/dm11;
CMatrix3DH m12(d1, y1, z1, 1.0, d2, y2, z2, 1.0, d3, y3, z3, 1.0, d4, y4, z4, 1.0);
double dm12 = m12.determinant();
CMatrix3DH m13(x1, d1, z1, 1.0, x2, d2, z2, 1.0, x3, d3, z3, 1.0, x4, d4, z4, 1.0);
double dm13 = m13.determinant();
CMatrix3DH m14(x1, y1, d1, 1.0, x2, y2, d2, 1.0, x3, y3, d3, 1.0, x4, y4, d4, 1.0);
double dm14 = m14.determinant();
CMatrix3DH m15(d1, x1, y1, z1, d2, x2, y2, z2, d3, x3, y3, z3, d4, x4, y4, z4);
double dm15 = m15.determinant();
*xc = 0.5*dm12*idm11;
*yc = 0.5*dm13*idm11;
*zc = 0.5*dm14*idm11;
*r = sqrt((*xc)*(*xc) + (*yc)*(*yc) + (*zc)*(*zc) - dm15*idm11);
return true;
}
CSSFunctionValue* MatrixTransformComponent::toCSSValue() const
{
CSSFunctionValue* result = CSSFunctionValue::create(m_is2D ? CSSValueMatrix : CSSValueMatrix3d);
if (m_is2D) {
double values[6] = {a(), b(), c(), d(), e(), f()};
for (double value : values) {
result->append(cssValuePool().createValue(value, CSSPrimitiveValue::UnitType::Number));
}
} else {
double values[16] = {m11(), m12(), m13(), m14(), m21(), m22(), m23(), m24(),
m31(), m32(), m33(), m34(), m41(), m42(), m43(), m44()
};
for (double value : values) {
result->append(cssValuePool().createValue(value, CSSPrimitiveValue::UnitType::Number));
}
}
return result;
}
TransformationMatrix::operator CATransform3D() const
{
CATransform3D toT3D;
toT3D.m11 = narrowPrecisionToFloat(m11());
toT3D.m12 = narrowPrecisionToFloat(m12());
toT3D.m13 = narrowPrecisionToFloat(m13());
toT3D.m14 = narrowPrecisionToFloat(m14());
toT3D.m21 = narrowPrecisionToFloat(m21());
toT3D.m22 = narrowPrecisionToFloat(m22());
toT3D.m23 = narrowPrecisionToFloat(m23());
toT3D.m24 = narrowPrecisionToFloat(m24());
toT3D.m31 = narrowPrecisionToFloat(m31());
toT3D.m32 = narrowPrecisionToFloat(m32());
toT3D.m33 = narrowPrecisionToFloat(m33());
toT3D.m34 = narrowPrecisionToFloat(m34());
toT3D.m41 = narrowPrecisionToFloat(m41());
toT3D.m42 = narrowPrecisionToFloat(m42());
toT3D.m43 = narrowPrecisionToFloat(m43());
toT3D.m44 = narrowPrecisionToFloat(m44());
return toT3D;
}
double WTransform::determinant() const
{
return m11() * (m33() * m22() - m32() * m23())
- m21() * (m33() * m12() - m32() * m13())
+ m31() * (m23() * m12() - m22() * m13());
}
