void GFXDrawUtil::draw2DSquare( const Point2F &screenPoint, F32 width, F32 spinAngle )
{
width *= 0.5;
Point3F offset( screenPoint.x, screenPoint.y, 0.0 );
GFXVertexBufferHandle<GFXVertexPC> verts( mDevice, 4, GFXBufferTypeVolatile );
verts.lock();
verts[0].point.set( -width, -width, 0.0f );
verts[1].point.set( -width, width, 0.0f );
verts[2].point.set( width, -width, 0.0f );
verts[3].point.set( width, width, 0.0f );
verts[0].color = verts[1].color = verts[2].color = verts[3].color = mBitmapModulation;
if(spinAngle != 0.f)
{
MatrixF rotMatrix( EulerF( 0.0, 0.0, spinAngle ) );
for( S32 i = 0; i < 4; i++ )
{
rotMatrix.mulP( verts[i].point );
verts[i].point += offset;
}
}
verts.unlock();
mDevice->setVertexBuffer( verts );
mDevice->setStateBlock(mRectFillSB);
mDevice->setupGenericShaders();
mDevice->drawPrimitive( GFXTriangleStrip, 0, 2 );
}
//---------------------------------------------------------------------------
bool MJ2000EcAxes::Initialize()
{
InertialAxes::Initialize();
// initialize the rotation matrix
rotMatrix(0,0) = 1.0;
rotMatrix(0,1) = 0.0;
rotMatrix(0,2) = 0.0;
rotMatrix(1,0) = 0.0;
rotMatrix(1,1) = 0.917482062076895741;
rotMatrix(1,2) = -0.397777155914121383;
rotMatrix(2,0) = 0.0;
rotMatrix(2,1) = 0.397777155914121383;
rotMatrix(2,2) = 0.917482062076895741;
// rotDotMatrix is still the default zero matrix
return true;
}
//---------------------------------------------------------------------------
bool MJ2000EqAxes::Initialize()
{
#ifdef DEBUG_CONSTRUCTION
MessageInterface::ShowMessage("Now entering MJ2000Eq INIT with name %s\n",
instanceName.c_str());
#endif
InertialAxes::Initialize();
// initialize the rotation matrix to be the identity matrix
rotMatrix(0,0) = 1.0;
rotMatrix(1,1) = 1.0;
rotMatrix(2,2) = 1.0;
// rotDotMatrix is still the default zero matrix
#ifdef DEBUG_CONSTRUCTION
MessageInterface::ShowMessage("Now LEAVING MJ2000Eq INIT with name %s\n",
instanceName.c_str());
#endif
return true;
}
void TurnAlways::simulateUntil(unsigned long t)
{
unsigned long dt = t - lastV; // how long since last update
float secs = ((float) dt) / 1000; // convert ms to sec
lastV = t;
Matrix delta, newT;
rotMatrix(delta,'Y',secs * vel);
multMatrix(delta,owner->transform,newT);
copyMatrix(newT,owner->transform);
lastV = t;
}
static void BuildJenga (DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int count)
{
// build a standard block stack of 20 * 3 boxes for a total of 60
NewtonWorld* const world = scene->GetNewton();
dVector blockBoxSize (0.8f, 0.5f, 0.8f * 3.0f);
blockBoxSize = blockBoxSize.Scale (1.0f);
// create the stack
dMatrix baseMatrix (dGetIdentityMatrix());
// for the elevation of the floor at the stack position
baseMatrix.m_posit.m_x = origin.m_x;
baseMatrix.m_posit.m_z = origin.m_z;
dFloat startElevation = 100.0f;
dVector floor (FindFloor (world, dVector (baseMatrix.m_posit.m_x, startElevation, baseMatrix.m_posit.m_z, 0.0f), 2.0f * startElevation));
baseMatrix.m_posit.m_y = floor.m_y + blockBoxSize.m_y / 2.0f;
// set realistic mass and inertia matrix for each block
mass = 5.0f;
// create a 90 degree rotation matrix
dMatrix rotMatrix (dYawMatrix (3.141592f * 0.5f));
// create a material to control collision with this objects
int defaultMaterialID;
defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
// separate a bit the block alone the horizontal direction
dFloat gap = 0.01f;
// create the shape and visual mesh as a common data to be re used
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), blockBoxSize, _BOX_PRIMITIVE, defaultMaterialID);
DemoMesh* const geometry = new DemoMesh("box", collision, "wood_0.tga", "wood_0.tga", "wood_0.tga");
for (int i = 0; i < count; i ++) {
dMatrix matrix(baseMatrix);
matrix.m_posit -= matrix.m_front.Scale (blockBoxSize.m_x - gap);
for (int j = 0; j < 3; j ++) {
CreateSimpleSolid (scene, geometry, mass, matrix, collision, defaultMaterialID);
matrix.m_posit += matrix.m_front.Scale (blockBoxSize.m_x + gap);
}
baseMatrix = rotMatrix * baseMatrix;
baseMatrix.m_posit += matrix.m_up.Scale (blockBoxSize.m_y * 0.99f);
}
// do not forget to release the assets
geometry->Release();
NewtonDestroyCollision (collision);
}
void Spin::simulateUntil(unsigned long t)
{
// remember position
float x = owner->transform[3][0];
float y = owner->transform[3][1];
float z = owner->transform[3][2];
// set rotation based on time
rotMatrix(owner->transform, 'y', vel * static_cast<float>(t));
// put the position back
owner->transform[3][0] = x;
owner->transform[3][1] = y;
owner->transform[3][2] = z;
lastV = t;
}
void tSpriteBatch::draw(uint8_t z, tTexture* t, const tRectf& dest, const tOptional<tRectf>& src, const tColor4f& c,
float rotz, const tPoint2f& origin)
{
std::pair<uint8_t, tTexture*> key(z, t);
Value& ref = mSteps[key];
size_t curIndex = ref.mVerts.size();
//Calculate position coords
FourPoints(ref.mVerts, dest);
float co = cosf(rotz);
float s = sinf(rotz);
tMatrix2x2f rotMatrix(tVector2f( co, -s),
tVector2f( s, co));
for (size_t i = 0; i < 4; i++)
{
tVector2f in = ref.mVerts[curIndex + i] - tVector2f(dest.location.x, dest.location.y) - tVector2f(origin.x, origin.y);
tVector2f out = in * rotMatrix;
ref.mVerts[curIndex + i] = out + tVector2f(dest.location.x, dest.location.y);
}
//Calculate texture coords
if (src)
{
tDimension2f texSize = tDimension2f(t->getSize().x, t->getSize().y);
FourPoints(ref.mTexCoords,
tRectf((float)src->location.x / texSize.width,
(float)src->location.y / texSize.height,
(float)src->size.width / texSize.width,
(float)src->size.height / texSize.height));
}
else
{
FourPoints(ref.mTexCoords, tRectf(0, 0,
(float)t->getSurfaceSize().width / (float)t->getSize().width,
(float)t->getSurfaceSize().height / (float)t->getSize().height));
}
//Calculate colors
ref.mColors.push_back(c); ref.mColors.push_back(c);
ref.mColors.push_back(c); ref.mColors.push_back(c);
//Calculate indices
SixIndices(ref.mIndices, (uint16_t)curIndex);
}
cv::Point3f GeometryUtils::rotateAroundAxis(const cv::Vec3f &vect, const cv::Vec3f &axis,
const double &theta) {
//Build the rotation matrix
double u = axis[0];
double v = axis[1];
double w = axis[2];
cv::Mat rotMatrix(4, 4, CV_64F);
rotMatrix.at<double>(0) = (u * u + (1 - u * u) * std::cos(theta));
rotMatrix.at<double>(1) = (u * v * (1 - std::cos(theta)) + w * std::sin(theta));
rotMatrix.at<double>(2) = (u * w * (1 - std::cos(theta)) - v * std::sin(theta));
rotMatrix.at<double>(3) = (0);
rotMatrix.at<double>(4) = (u * v * (1 - std::cos(theta)) - w * std::sin(theta));
rotMatrix.at<double>(5) = (v * v + (1 - v * v) * std::cos(theta));
rotMatrix.at<double>(6) = (v * w * (1 - std::cos(theta)) + u * std::sin(theta));
rotMatrix.at<double>(7) = (0);
rotMatrix.at<double>(8) = (u * w * (1 - std::cos(theta)) + v * std::sin(theta));
rotMatrix.at<double>(9) = (v * w * (1 - std::cos(theta)) - u * std::sin(theta));
rotMatrix.at<double>(10) = (w * w + (1 - w * w) * std::cos(theta));
rotMatrix.at<double>(11) = (0);
rotMatrix.at<double>(12) = (0);
rotMatrix.at<double>(13) = (0);
rotMatrix.at<double>(14) = (0);
rotMatrix.at<double>(15) = (1);
cv::Point3f result;
result.x = rotMatrix.at<double>(0) * vect[0] +
rotMatrix.at<double>(1) * vect[1] +
rotMatrix.at<double>(2) * vect[2] +
rotMatrix.at<double>(3);
result.y = rotMatrix.at<double>(4) * vect[0] +
rotMatrix.at<double>(5) * vect[1] +
rotMatrix.at<double>(6) * vect[2] +
rotMatrix.at<double>(7);
result.z = rotMatrix.at<double>(8) * vect[0] +
rotMatrix.at<double>(9) * vect[1] +
rotMatrix.at<double>(10) * vect[2] +
rotMatrix.at<double>(11);
return result;
}
//--------------------------------------------------------------------------
// Update
//--------------------------------------------------------------------------
void CameraShake::update( F32 dt )
{
Parent::update( dt );
fadeAmplitude();
VectorF camOffset;
camOffset.x = mAmp.x * sin( M_2PI * (mTimeOffset.x + mElapsedTime) * mFreq.x );
camOffset.y = mAmp.y * sin( M_2PI * (mTimeOffset.y + mElapsedTime) * mFreq.y );
camOffset.z = mAmp.z * sin( M_2PI * (mTimeOffset.z + mElapsedTime) * mFreq.z );
VectorF rotAngles;
rotAngles.x = camOffset.x * 10.0 * M_PI/180.0;
rotAngles.y = camOffset.y * 10.0 * M_PI/180.0;
rotAngles.z = camOffset.z * 10.0 * M_PI/180.0;
MatrixF rotMatrix( EulerF( rotAngles.x, rotAngles.y, rotAngles.z ) );
mCamFXTrans = rotMatrix;
mCamFXTrans.setPosition( camOffset );
}
void dComplentaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
{
const dFloat D_MAX_ANGLE_STEP = dFloat (45.0f * 3.141592f / 180.0f);
const dFloat D_ANGULAR_TOL = dFloat (0.0125f * 3.141592f / 180.0f);
m_globalCentreOfMass += m_veloc.Scale (timestep);
while (((m_omega % m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
m_omega = m_omega.Scale (dFloat (0.8f));
}
// this is correct
dFloat omegaMag2 = m_omega % m_omega;
if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (m_omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion rotation (omegaAxis, omegaAngle);
dQuaternion rotMatrix (m_matrix);
rotMatrix = rotMatrix * rotation;
rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);
#ifdef _DEBUG
int j0 = 1;
int j1 = 2;
for (int i = 0; i < 3; i ++) {
dAssert (m_matrix[i][3] == 0.0f);
dFloat val = m_matrix[i] % m_matrix[i];
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
dVector tmp (m_matrix[j0] * m_matrix[j1]);
val = tmp % m_matrix[i];
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
//---------------------------------------------------------------------------
void ObjectReferencedAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
bool forceComputation)
{
if (!primary)
throw CoordinateSystemException("Primary \"" + primaryName +
"\" is not yet set in object referenced coordinate system!");
if (!secondary)
throw CoordinateSystemException("Secondary \"" + secondaryName +
"\" is not yet set in object referenced coordinate system!");
if ((xAxis == yAxis) || (xAxis == zAxis) || (yAxis == zAxis))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, axes are improperly "
"defined.\nXAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
if ((xAxis != "") && (yAxis != "") && (zAxis != ""))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, too many axes are defined.\n"
"XAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
SpacePoint *useAsSecondary = secondary;
// if (!useAsSecondary) useAsSecondary = origin;
Rvector6 rv = useAsSecondary->GetMJ2000State(atEpoch) -
primary->GetMJ2000State(atEpoch);
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
MessageInterface::ShowMessage(" ------------ rv Primary (%s) to Secondary (%s) = %s\n",
primary->GetName().c_str(), secondary->GetName().c_str(), rv.ToString().c_str());
visitCount++;
}
#endif
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
std::stringstream ss;
ss.precision(30);
ss << " ----------------- rv Earth to Moon (truncated) = "
<< rv << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
visitCount++;
}
#endif
Rvector3 a = useAsSecondary->GetMJ2000Acceleration(atEpoch) -
primary->GetMJ2000Acceleration(atEpoch);
Rvector3 r = rv.GetR();
Rvector3 v = rv.GetV();
Rvector3 n = Cross(r,v);
Rvector3 rUnit = r.GetUnitVector();
Rvector3 vUnit = v.GetUnitVector();
Rvector3 nUnit = n.GetUnitVector();
Real rMag = r.GetMagnitude();
Real vMag = v.GetMagnitude();
Real nMag = n.GetMagnitude();
// check for divide-by-zero
if ((GmatMathUtil::IsEqual(rMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(vMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(nMag, MAGNITUDE_TOL)))
{
std::string errmsg = "Object referenced axis system named \"";
errmsg += coordName + "\" is undefined because at least one axis is near zero in length.\n";
throw CoordinateSystemException(errmsg);
}
Rvector3 rDot = (v / rMag) - (rUnit / rMag) * (rUnit * v);
Rvector3 vDot = (a / vMag) - (vUnit / vMag) * (vUnit * a);
Rvector3 nDot = (Cross(r,a) / nMag) - (nUnit / nMag) * (Cross(r,a) * nUnit);
Rvector3 xUnit, yUnit, zUnit, xDot, yDot, zDot;
bool xUsed = true, yUsed = true, zUsed = true;
// determine the x-axis
if ((xAxis == "R") || (xAxis == "r"))
{
xUnit = rUnit;
xDot = rDot;
}
else if ((xAxis == "-R") || (xAxis == "-r"))
{
xUnit = -rUnit;
xDot = -rDot;
}
else if ((xAxis == "V") || (xAxis == "v"))
{
xUnit = vUnit;
xDot = vDot;
}
else if ((xAxis == "-V") || (xAxis == "-v"))
{
xUnit = -vUnit;
xDot = -vDot;
}
else if ((xAxis == "N") || (xAxis == "n"))
{
xUnit = nUnit;
xDot = nDot;
}
else if ((xAxis == "-N") || (xAxis == "-n"))
{
xUnit = -nUnit;
xDot = -nDot;
}
else
{
xUsed = false;
}
// determine the y-axis
if ((yAxis == "R") || (yAxis == "r"))
{
yUnit = rUnit;
yDot = rDot;
}
else if ((yAxis == "-R") || (yAxis == "-r"))
{
yUnit = -rUnit;
yDot = -rDot;
}
else if ((yAxis == "V") || (yAxis == "v"))
{
yUnit = vUnit;
yDot = vDot;
}
else if ((yAxis == "-V") || (yAxis == "-v"))
{
yUnit = -vUnit;
yDot = -vDot;
}
else if ((yAxis == "N") || (yAxis == "n"))
{
yUnit = nUnit;
yDot = nDot;
}
else if ((yAxis == "-N") || (yAxis == "-n"))
{
yUnit = -nUnit;
yDot = -nDot;
}
else
{
yUsed = false;
}
// determine the z-axis
if ((zAxis == "R") || (zAxis == "r"))
{
zUnit = rUnit;
zDot = rDot;
}
else if ((zAxis == "-R") || (zAxis == "-r"))
{
zUnit = -rUnit;
zDot = -rDot;
}
else if ((zAxis == "V") || (zAxis == "v"))
{
zUnit = vUnit;
zDot = vDot;
}
else if ((zAxis == "-V") || (zAxis == "-v"))
{
zUnit = -vUnit;
zDot = -vDot;
}
else if ((zAxis == "N") || (zAxis == "n"))
{
zUnit = nUnit;
zDot = nDot;
}
else if ((zAxis == "-N") || (zAxis == "-n"))
{
zUnit = -nUnit;
zDot = -nDot;
}
else
{
zUsed = false;
}
// determine the third axis
if (xUsed && yUsed && !zUsed)
{
zUnit = Cross(xUnit, yUnit);
zDot = Cross(xDot, yUnit) + Cross(xUnit, yDot);
}
else if (xUsed && zUsed && !yUsed)
{
yUnit = Cross(zUnit,xUnit);
yDot = Cross(zDot, xUnit) + Cross(zUnit, xDot);
}
else if (yUsed && zUsed && !xUsed)
{
xUnit = Cross(yUnit,zUnit);
xDot = Cross(yDot, zUnit) + Cross(yUnit, zDot);
}
else
{
throw CoordinateSystemException(
"Object referenced axes are improperly defined.");
}
// Compute the rotation matrix
rotMatrix(0,0) = xUnit(0);
rotMatrix(0,1) = yUnit(0);
rotMatrix(0,2) = zUnit(0);
rotMatrix(1,0) = xUnit(1);
rotMatrix(1,1) = yUnit(1);
rotMatrix(1,2) = zUnit(1);
rotMatrix(2,0) = xUnit(2);
rotMatrix(2,1) = yUnit(2);
rotMatrix(2,2) = zUnit(2);
// Compute the rotation derivative matrix
rotDotMatrix(0,0) = xDot(0);
rotDotMatrix(0,1) = yDot(0);
rotDotMatrix(0,2) = zDot(0);
rotDotMatrix(1,0) = xDot(1);
rotDotMatrix(1,1) = yDot(1);
rotDotMatrix(1,2) = zDot(1);
rotDotMatrix(2,0) = xDot(2);
rotDotMatrix(2,1) = yDot(2);
rotDotMatrix(2,2) = zDot(2);
#ifdef DEBUG_ROT_MATRIX
MessageInterface::ShowMessage
("rotMatrix=%s\n", rotMatrix.ToString().c_str());
std::stringstream ss;
ss.setf(std::ios::fixed);
ss.precision(30);
ss << " ----------------- rotMatrix = " << rotMatrix << std::endl;
ss.setf(std::ios::scientific);
ss << " ----------------- rotDotMatrix = " << rotDotMatrix << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
#endif
if (!rotMatrix.IsOrthonormal(ORTHONORMAL_TOL))
{
std::stringstream errmsg("");
errmsg << "*** WARNING*** Object referenced axis system \"" << coordName;
errmsg << "\" has a non-orthogonal rotation matrix. " << std::endl;
}
}
void XsiExp::ExportMesh( INode * node, TimeValue t, int indentLevel)
{
ObjectState os = node->EvalWorldState(t);
if (!os.obj || os.obj->SuperClassID() != GEOMOBJECT_CLASS_ID)
{
return; // Safety net. This shouldn't happen.
}
BOOL needDel;
TriObject * tri = GetTriObjectFromNode(node, t, needDel);
if (!tri)
{
// no tri object
return;
}
// prepare mesh
Mesh * mesh = &tri->GetMesh();
mesh->buildNormals();
// object offset matrix; apply to verts
// swap y and z; max to soft correction
Matrix3 matrix(1);
// translate
matrix.PreTranslate( Point3( node->GetObjOffsetPos().x, node->GetObjOffsetPos().z, -node->GetObjOffsetPos().y));
// rotate
AngAxis aa( node->GetObjOffsetRot());
float temp = aa.axis.z;
aa.axis.z = -aa.axis.y;
aa.axis.y = temp;
PreRotateMatrix(matrix, Quat( aa));
// scale
ScaleValue scale = node->GetObjOffsetScale();
aa.Set( scale.q);
temp = aa.axis.z;
aa.axis.z = -aa.axis.y;
aa.axis.y = temp;
scale.q.Set( aa);
temp = scale.s.z;
scale.s.z = scale.s.y;
scale.s.y = temp;
ApplyScaling(matrix, scale);
// apply root transform
matrix = matrix * topMatrix;
// only rotation for normals
AffineParts ap;
Matrix3 rotMatrix(1);
decomp_affine( matrix, &ap);
PreRotateMatrix( rotMatrix, ap.q);
// set winding order
int vx1 = 0, vx2 = 1, vx3 = 2;
if (TMNegParity( node->GetNodeTM(GetStaticFrame())) != TMNegParity( matrix) )
{
// negative scaling; invert winding order and normal rotation
vx1 = 2; vx2 = 1; vx3 = 0;
rotMatrix = rotMatrix * Matrix3( Point3(-1,0,0), Point3(0,-1,0), Point3(0,0,-1), Point3(0,0,0));
}
// header
TSTR indent = GetIndent(indentLevel+1);
fprintf(pStream, "%s%s %s {\n",indent.data(), "Mesh", FixupName(node->GetName()));
// write number of verts
int numLoop = mesh->getNumVerts();
fprintf(pStream, "%s\t%d;\n",indent.data(), numLoop);
// write verts
for (int i = 0; i < numLoop; i++)
{
Point3 v = mesh->verts[i];
float temp = v.z;
v.z = -v.y;
v.y = temp;
v = matrix * v;
fprintf(pStream, "%s\t%.6f;%.6f;%.6f;%s\n", indent.data(), v.x, v.y, v.z,
i == numLoop - 1 ? ";\n" : ",");
}
// write number of faces
numLoop = mesh->getNumFaces();
fprintf(pStream, "%s\t%d;\n", indent.data(), numLoop);
// write faces
for (i = 0; i < numLoop; i++)
{
fprintf(pStream, "%s\t3;%d,%d,%d;%s\n",
indent.data(),
mesh->faces[i].v[vx1],
mesh->faces[i].v[vx2],
mesh->faces[i].v[vx3],
i == numLoop - 1 ? ";\n" : ",");
}
// face materials
Mtl * nodeMtl = node->GetMtl();
int numMtls = !nodeMtl || !nodeMtl->NumSubMtls() ? 1 : nodeMtl->NumSubMtls();
// write face material list header
fprintf(pStream, "%s\tMeshMaterialList {\n", indent.data());
// write number of materials
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numMtls);
// write number of faces
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop);
// write face material indices (1 for each face)
for (i = 0; i < numLoop; i++)
{
int index = numMtls ? mesh->faces[i].getMatID() % numMtls : 0;
fprintf(pStream,"%s\t\t%d%s\n",
indent.data(),
index,
i == numLoop - 1 ? ";\n" : ",");
}
// write the materials
ExportMaterial( node, indentLevel+2);
// verts close brace
fprintf(pStream, "%s\t}\n\n",indent.data());
// write normals header
fprintf(pStream, "%s\t%s {\n", indent.data(), "SI_MeshNormals");
// write number of normals
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop * 3);
// write normals (3 for each face)
for (i = 0; i < numLoop; i++)
{
Face * f = &mesh->faces[i];
int vert = f->getVert(vx1);
Point3 vn = GetVertexNormal(mesh, i, mesh->getRVertPtr(vert));
float temp = vn.z;
vn.z = -vn.y;
vn.y = temp;
vn = rotMatrix * vn;
fprintf(pStream,"%s\t\t%.6f;%.6f;%.6f;,\n", indent.data(), vn.x, vn.y, vn.z);
vert = f->getVert(vx2);
vn = GetVertexNormal(mesh, i, mesh->getRVertPtr(vert));
temp = vn.z;
vn.z = -vn.y;
vn.y = temp;
vn = rotMatrix * vn;
fprintf(pStream,"%s\t\t%.6f;%.6f;%.6f;,\n", indent.data(), vn.x, vn.y, vn.z);
vert = f->getVert(vx3);
vn = GetVertexNormal(mesh, i, mesh->getRVertPtr(vert));
temp = vn.z;
vn.z = -vn.y;
vn.y = temp;
vn = rotMatrix * vn;
fprintf(pStream,"%s\t\t%.6f;%.6f;%.6f;%s\n", indent.data(), vn.x, vn.y, vn.z,
i == numLoop - 1 ? ";\n" : ",");
}
// write number of faces
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop);
// write faces
for (i = 0; i < numLoop; i++)
{
fprintf(pStream, "%s\t\t%d;3;%d,%d,%d;%s\n",
indent.data(),
i,
i * 3 + vx1, i * 3 + vx2, i * 3 + vx3,
i == numLoop - 1 ? ";\n" : ",");
}
// normals close brace
fprintf(pStream, "%s\t}\n\n",indent.data());
// texcoords
if (nodeMtl && mesh && (nodeMtl->Requirements(-1) & MTLREQ_FACEMAP))
{
// facemapping
numLoop = mesh->getNumFaces() * 3;
// write texture coords header
fprintf(pStream, "%s\tSI_MeshTextureCoords {\n", indent.data());
// write number of texture coords
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop);
// write texture coords
for (int i = 0; i < numLoop; i++)
{
Point3 tv[3];
Face * f = &mesh->faces[i];
make_face_uv( f, tv);
fprintf(pStream, "%s\t\t%.6f;%.6f;,\n", indent.data(), tv[0].x, tv[0].y);
fprintf(pStream, "%s\t\t%.6f;%.6f;,\n", indent.data(), tv[1].x, tv[1].y);
fprintf(pStream, "%s\t\t%.6f;%.6f;%s\n", indent.data(), tv[2].x, tv[2].y,
i == numLoop - 1 ? ";\n" : ",");
}
// write number of faces
numLoop = mesh->getNumFaces();
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop);
// write faces
for (i = 0; i < numLoop; i++)
{
fprintf(pStream,"%s\t\t%d;3;%d,%d,%d;%s\n",
indent.data(),
i,
mesh->tvFace[i].t[vx1],
mesh->tvFace[i].t[vx2],
mesh->tvFace[i].t[vx3],
i == numLoop - 1 ? ";\n" : ",");
}
// texture coords close brace
fprintf(pStream, "%s\t}\n\n", indent.data());
}
else
{
numLoop = mesh->getNumTVerts();
if (numLoop)
{
// write texture coords header
fprintf(pStream, "%s\tSI_MeshTextureCoords {\n", indent.data());
// write number of texture coords
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop);
// write texture coords
for (i = 0; i < numLoop; i++)
{
UVVert tv = mesh->tVerts[i];
fprintf(pStream, "%s\t\t%.6f;%.6f;%s\n", indent.data(), tv.x, tv.y,
i == numLoop - 1 ? ";\n" : ",");
}
// write number of faces
numLoop = mesh->getNumFaces();
fprintf(pStream, "%s\t\t%d;\n", indent.data(), numLoop);
// write faces
for (i = 0; i < numLoop; i++)
{
fprintf(pStream,"%s\t\t%d;3;%d,%d,%d;%s\n",
indent.data(),
i,
mesh->tvFace[i].t[vx1],
mesh->tvFace[i].t[vx2],
mesh->tvFace[i].t[vx3],
i == numLoop - 1 ? ";\n" : ",");
}
// texture coords close brace
fprintf(pStream, "%s\t}\n\n", indent.data());
}
}
/*
// Export color per vertex info
if (GetIncludeVertexColors()) {
int numCVx = mesh->numCVerts;
fprintf(pStream, "%s\t%s %d\n",indent.data(), ID_MESH_NUMCVERTEX, numCVx);
if (numCVx) {
fprintf(pStream,"%s\t%s {\n",indent.data(), ID_MESH_CVERTLIST);
for (i=0; i<numCVx; i++) {
Point3 vc = mesh->vertCol[i];
fprintf(pStream, "%s\t\t%s %d\t%s\n",indent.data(), ID_MESH_VERTCOL, i, Format(vc));
}
fprintf(pStream,"%s\t}\n",indent.data());
fprintf(pStream, "%s\t%s %d\n",indent.data(), ID_MESH_NUMCVFACES, mesh->getNumFaces());
fprintf(pStream, "%s\t%s {\n",indent.data(), ID_MESH_CFACELIST);
for (i=0; i<mesh->getNumFaces(); i++) {
fprintf(pStream,"%s\t\t%s %d\t%d\t%d\t%d\n",
indent.data(),
ID_MESH_CFACE, i,
mesh->vcFace[i].t[vx1],
mesh->vcFace[i].t[vx2],
mesh->vcFace[i].t[vx3]);
}
fprintf(pStream, "%s\t}\n",indent.data());
}
}
*/
// Mesh close brace
fprintf(pStream, "%s}\n",indent.data());
// dispose of tri object
if (needDel)
{
delete tri;
}
}
void cvbundler::readOutputFile(string filename) {
ifstream outputFile;
outputFile.open(filename.c_str());
std::string word;
getline(outputFile, word); // get header line
outputFile >> word;
int nCameras = atoi(word.c_str());
std::cout << "Number of cameras" << nCameras << std::endl;
outputFile >> word;
int nPoints = atoi(word.c_str());
std::cout << "Number of points" << nPoints << std::endl;
_rvecs.resize(nCameras);
_tvecs.resize(nCameras);
for(int c=0; c<nCameras; c++) {
std::cout << "Cam " << c << std::endl;
outputFile >> word;
std::cout << "Focal length" << atof(word.c_str()) << std::endl;
outputFile >> word;
std::cout << "Dist 1 " << atof(word.c_str()) << std::endl;
outputFile >> word;
std::cout << "Dist 2" << atof(word.c_str()) << std::endl;
cv::Mat rotMatrix(3, 3, CV_32FC1);
for(int i=0; i<9; i++) {
outputFile >> word;
rotMatrix.ptr<float>()[i] = atof(word.c_str());
}
cv::Rodrigues(rotMatrix, _rvecs[c]);
_tvecs[c] = Mat(3, 1, CV_32FC1);
for(int i=0; i<3; i++) {
outputFile >> word;
_tvecs[c].ptr<float>()[i] = atof(word.c_str());
}
std::cout << _tvecs[c] << std::endl;
}
_points3d.resize(nPoints);
_pointsColor.resize(nPoints);
for(int p=0; p<nPoints; p++) {
outputFile >> word;
_points3d[p].x = atof(word.c_str());
outputFile >> word;
_points3d[p].y = atof(word.c_str());
outputFile >> word;
_points3d[p].z = atof(word.c_str());
outputFile >> word;
_pointsColor[p].val[0] = atoi(word.c_str());
outputFile >> word;
_pointsColor[p].val[1] = atoi(word.c_str());
outputFile >> word;
_pointsColor[p].val[2] = atoi(word.c_str());
outputFile >> word;
getline(outputFile, word); // ignore view list line
}
outputFile.close();
}
void DrawCoordinateSystem::projectPoints(){
cv::Mat_<double> rvec(3,1);
cv::Mat_<double> tvec(3,1);
Types::HomogMatrix homogMatrix;
cv::Mat_<double> rotMatrix;
rotMatrix.create(3,3);
// Try to read rotation and translation from rvec and tvec or homogenous matrix.
if(!in_rvec.empty()&&!in_tvec.empty()){
rvec = in_rvec.read();
tvec = in_tvec.read();
}
else if(!in_homogMatrix.empty()){
homogMatrix = in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
}
else
return;
// Get camera info properties.
Types::CameraInfo camera_matrix = in_camera_matrix.read();
vector<cv::Point3f> object_points;
vector<cv::Point2f> image_points;
// Create four points constituting ends of three lines.
object_points.push_back(cv::Point3f(0,0,0));
object_points.push_back(cv::Point3f(0.1,0,0));
object_points.push_back(cv::Point3f(0,0.1,0));
object_points.push_back(cv::Point3f(0,0,0.1));
// Project points taking into account the camera properties.
if (rectified) {
cv::Mat K, _r, _t;
cv::decomposeProjectionMatrix(camera_matrix.projectionMatrix(), K, _r, _t);
cv::projectPoints(object_points, rvec, tvec, K, cv::Mat(), image_points);
} else {
cv::projectPoints(object_points, rvec, tvec, camera_matrix.cameraMatrix(), camera_matrix.distCoeffs(), image_points);
}
// Draw lines between projected points.
Types::Line ax(image_points[0], image_points[1]);
ax.setCol(cv::Scalar(255, 0, 0));
Types::Line ay(image_points[0], image_points[2]);
ay.setCol(cv::Scalar(0, 255, 0));
Types::Line az(image_points[0], image_points[3]);
az.setCol(cv::Scalar(0, 0, 255));
// Add lines to container.
Types::DrawableContainer ctr;
ctr.add(ax.clone());
ctr.add(ay.clone());
ctr.add(az.clone());
CLOG(LINFO)<<"Image Points: "<<image_points;
// Write output to dataports.
out_csystem.write(ctr);
out_impoints.write(image_points);
}
/* simplest possible house */
SimpleHouse2::SimpleHouse2()
{
color(.6f,.7f,.8f);
rotMatrix(transform,'Y',3.14159f/2.f);
}
/**
@note scaling DOES propagate to children
*/
void IFXCharacter::ForEachNodeTransformed2(I32 flags,IFXCoreNode &rParent,
IFXForEachNodeTransformedCB callback,IFXVariant state)
{
IFXBoneNodeList &rChildlist=rParent.Children();
if(!rChildlist.GetNumberElements())
return;
IFXBoneNode *pNode = NULL;
IFXListContext context;
BOOL stop=false;
rChildlist.ToHead(context);
while( (pNode=rChildlist.PostIncrement(context)) != NULL)
{
F32 length=pNode->GetLength();
m_transforms.Push();
// I've changed following code (commented out now) because I'm not sure
// about IFXTransformation class quality.
// m_transforms.GetCurrent().Translate(pNode->DisplacementConst());
// m_transforms.GetCurrent().Rotate(pNode->RotationConst());
// m_transforms.GetCurrent().Scale(pNode->ScaleConst());
IFXMatrix4x4 matrix; // for current transformation
IFXMatrix4x4 parentMatrix = m_transforms.GetCurrent().GetMatrixData(READONLY);
// add displacement
{
IFXMatrix4x4 temp;
temp.MakeIdentity();
temp.SetTranslation(pNode->DisplacementConst());
matrix.Multiply(parentMatrix, temp);
}
// add scale
matrix.Scale(pNode->ScaleConst());
//rotate
{
IFXMatrix4x4 temp;
IFXVector3 v3_temp;
v3_temp.X() = matrix[12];
v3_temp.Y() = matrix[13];
v3_temp.Z() = matrix[14];
matrix.ResetTranslation(); // translate to origin
IFXMatrix4x4 rotMatrix(pNode->RotationConst());
temp = matrix;
matrix.Multiply(temp, rotMatrix);
matrix.SetTranslation(v3_temp); // translate back
}
m_transforms.GetCurrent() = matrix;
if( !(flags&IFXCHILDFIRST) && callback)
stop=callback(*pNode,m_transforms.GetCurrent(),state);
if(flags&IFXSTORE_XFORM)
pNode->StoreTransform(m_transforms.GetCurrent());
//was: m_transforms.GetCurrent().Translate(length,0.0f,0.0f);
{
IFXMatrix4x4 temp;
temp.MakeIdentity();
temp[12] = length;
m_transforms.GetCurrent().Multiply(temp);
}
// recursively to children
if(!stop)
ForEachNodeTransformed2(flags,*pNode,callback,state);
if( (flags&IFXCHILDFIRST) && callback)
{
m_transforms.GetCurrent().Translate(-length,0.0f,0.0f);
callback(*pNode,m_transforms.GetCurrent(),state);
}
m_transforms.Pop();
}
}
void CalcStatistics::calculate() {
CLOG(LDEBUG)<<"in calculate()";
Types::HomogMatrix homogMatrix;
cv::Mat_<double> rvec;
cv::Mat_<double> tvec;
cv::Mat_<double> axis;
cv::Mat_<double> rotMatrix;
float fi;
rotMatrix= cv::Mat_<double>::zeros(3,3);
tvec = cv::Mat_<double>::zeros(3,1);
axis = cv::Mat_<double>::zeros(3,1);
// first homogMatrix on InputStream
if(counter == 0) {
homogMatrix = in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
cumulatedHomogMatrix = homogMatrix;
cumulatedTvec = tvec;
cumulatedRvec = rvec;
fi = sqrt((pow(rvec(0,0), 2) + pow(rvec(1,0), 2)+pow(rvec(2,0),2)));
cumulatedFi = fi;
for(int k=0;k<3;k++) {
axis(k,0)=rvec(k,0)/fi;
}
cumulatedAxis = axis;
counter=1;
return;
}
homogMatrix=in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
fi = sqrt((pow(rvec(0,0), 2) + pow(rvec(1,0), 2)+pow(rvec(2,0),2)));
for(int k=0;k<3;k++) {
axis(k,0)=rvec(k,0)/fi;
}
cumulatedFi += fi;
cumulatedTvec += tvec;
cumulatedRvec += rvec;
cumulatedAxis += axis;
counter++;
avgFi = cumulatedFi/counter;
avgAxis = cumulatedAxis/counter;
avgRvec = avgAxis * avgFi;
avgTvec = cumulatedTvec/counter;
Types::HomogMatrix hm;
cv::Mat_<double> rottMatrix;
Rodrigues(avgRvec, rottMatrix);
CLOG(LINFO)<<"Uśredniona macierz z "<<counter<<" macierzy \n";
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
hm(i, j) = rottMatrix(i, j);
CLOG(LINFO) << hm(i, j) << " ";
}
hm(i, 3) = avgTvec(i, 0);
CLOG(LINFO) << hm(i, 3) <<" \n";
}
out_homogMatrix.write(hm);
CLOG(LINFO)<<"Uśredniony kąt: " << avgFi;
float standardDeviationFi = sqrt(pow(avgFi - fi, 2));
CLOG(LINFO)<<"Odchylenie standardowe kąta: "<<standardDeviationFi;
}
void CalcObjectLocation::calculate() {
CLOG(LDEBUG)<<"in calculate()";
Types::HomogMatrix homogMatrix;
vector<cv::Mat_<double> > rvec;
vector<cv::Mat_<double> > tvec;
vector<cv::Mat_<double> > axis;
vector<double> fi;
cv::Mat_<double> tvectemp;
cv::Mat_<double> rotMatrix;
rotMatrix = cv::Mat_<double>::zeros(3,3);
tvectemp = cv::Mat_<double>::zeros(3,1);
while (!in_homogMatrix.empty()) {
cv::Mat_<double> rvectemp;
homogMatrix=in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix.elements[i][j];
}
tvectemp(i, 0) = homogMatrix.elements[i][3];
}
Rodrigues(rotMatrix, rvectemp);
CLOG(LINFO) << rvectemp << "\n";
rvec.push_back(rvectemp);
tvec.push_back(tvectemp);
}
if (rvec.size() == 1) {
out_homogMatrix.write(homogMatrix);
return;
}
float fi_sum=0, fi_avg;
cv::Mat_<double> axis_sum, axis_avg;
cv::Mat_<double> rvec_avg;
cv::Mat_<double> tvec_avg, tvec_sum;
axis_sum = cv::Mat_<double>::zeros(3,1);
tvec_sum = cv::Mat_<double>::zeros(3,1);
for(int i = 0; i < rvec.size(); i++) {
float fitmp = sqrt((pow(rvec.at(i)(0,0), 2) + pow(rvec.at(i)(1,0), 2)+pow(rvec.at(i)(2,0),2)));
fi.push_back(fitmp);
fi_sum+=fitmp;
cv::Mat_<double> axistemp;
axistemp.create(3,1);
for(int k=0;k<3;k++) {
axistemp(k,0)=rvec.at(i)(k,0)/fitmp;
}
axis.push_back(axistemp);
axis_sum+=axistemp;
tvec_sum+=tvec.at(i);
}
fi_avg = fi_sum/fi.size();
axis_avg = axis_sum/axis.size();
rvec_avg = axis_avg * fi_avg;
tvec_avg = tvec_sum/tvec.size();
Types::HomogMatrix hm;
cv::Mat_<double> rottMatrix;
Rodrigues(rvec_avg, rottMatrix);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
hm.elements[i][j] = rottMatrix(i, j);
CLOG(LINFO) << hm.elements[i][j] << " ";
}
hm.elements[i][3] = tvec_avg(i, 0);
CLOG(LINFO) << hm.elements[i][3] << "\n";
}
out_homogMatrix.write(hm);
}
SbXipImage* SoXipLoadBMP::loadBMP(const char *fileName)
{
try
{
BMP bmp;
bmp.ReadFromFile(fileName);
int numBits = bmp.TellBitDepth();
int width = bmp.TellWidth();
int height = bmp.TellHeight();
int bitsStored = 8;
int samplesPerPixel = 0;
SbXipImage::ComponentType compType = SbXipImage::INTERLEAVED;
SbXipImage::ComponentLayoutType compLayoutType = SbXipImage::RGB;
if(numBits<=24)
{
samplesPerPixel = 3;
compLayoutType = SbXipImage::RGB;
}
else if(numBits == 32)
{
samplesPerPixel = 4;
compLayoutType = SbXipImage::RGBA;
}
SbVec3f pos(0, 0, 0);
SbVec3f scale(width, height, 1);
SbMatrix rotMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1);
SbMatrix modelMatrix;
modelMatrix.setScale(scale);
//modelMatrix.setTransform(pos, SbRotation(rotMatrix), scale);
SbXipImage *image = new SbXipImage(SbXipImageDimensions(width, height, 1), SbXipImageDimensions(width, height, 1),
SbXipImage::UNSIGNED_BYTE, bitsStored, samplesPerPixel, compType, compLayoutType, modelMatrix);
unsigned char *buffer = (unsigned char *) image->refBufferPtr();
if(numBits<=24)
{
for(int i = 0; i < height; i++)
{
for(int j = 0; j < width; j++)
{
RGBApixel pixel = bmp.GetPixel(j,(height -1) - i );
buffer[3*i*width+3*j] = pixel.Red;
buffer[3*i*width+3*j+1] = pixel.Green;
buffer[3*i*width+3*j+2] = pixel.Blue;
}
}
}
else if(numBits == 32)
{
for(int i=0; i<height; i++)
{
for(int j=0; j<width; j++)
{
RGBApixel pixel = bmp.GetPixel(j, height-i);
buffer[4*i*width+4*j] = pixel.Red;
buffer[4*i*width+4*j+1] = pixel.Green;
buffer[4*i*width+4*j+2] = pixel.Blue;
buffer[4*i*width+4*j+3] = pixel.Alpha;
}
}
}
image->unrefBufferPtr();
return image;
}
catch(...)
{
SoDebugError::postInfo(__FILE__, "Exception loadBMP");
// Fix me need to delete allocated memory!
return 0;
}
}
