SbMatrix tgf::MatrixFromTransform( const Transform& transform )
{
Ptr<Matrix4x4> transformMatrix = transform.GetMatrix()->Transpose();
float m00 = float ( transformMatrix->m[0][0] );
float m01 = float ( transformMatrix->m[0][1] );
float m02 = float ( transformMatrix->m[0][2] );
float m03 = float ( transformMatrix->m[0][3] );
float m10 = float ( transformMatrix->m[1][0] );
float m11 = float ( transformMatrix->m[1][1] );
float m12 = float ( transformMatrix->m[1][2] );
float m13 = float ( transformMatrix->m[1][3] );
float m20 = float ( transformMatrix->m[2][0] );
float m21 = float ( transformMatrix->m[2][1] );
float m22 = float ( transformMatrix->m[2][2] );
float m23 = float ( transformMatrix->m[2][3] );
float m30 = float ( transformMatrix->m[3][0] );
float m31 = float ( transformMatrix->m[3][1] );
float m32 = float ( transformMatrix->m[3][2] );
float m33 = float ( transformMatrix->m[3][3] );
SbVec3f axis1( m00, m10, m20 );
SbVec3f axis2( m01, m11, m21 );
//axis2.normalize();
SbVec3f axis3( m02, m12, m22 );
//axis3.normalize();
return SbMatrix( axis1[0], axis2[0], axis3[0], m03,
axis1[1], axis2[1], axis3[1], m13,
axis1[2], axis2[2], axis3[2], m23,
m30, m31, m32, m33 );
}
bool IfcGeom::convert(const Ifc2x3::IfcCartesianTransformationOperator3DnonUniform::ptr l, gp_GTrsf& gtrsf) {
IN_CACHE(IfcCartesianTransformationOperator3DnonUniform,l,gp_GTrsf,gtrsf)
gp_Trsf trsf;
gp_Pnt origin;
IfcGeom::convert(l->LocalOrigin(),origin);
gp_Dir axis1 (1.,0.,0.);
gp_Dir axis2 (0.,1.,0.);
gp_Dir axis3;
if ( l->hasAxis1() ) IfcGeom::convert(l->Axis1(),axis1);
if ( l->hasAxis2() ) IfcGeom::convert(l->Axis2(),axis2);
if ( l->hasAxis3() ) IfcGeom::convert(l->Axis3(),axis3);
else axis3 = axis1.Crossed(axis2);
gp_Ax3 ax3 (origin,axis3,axis1);
if ( axis2.Dot(ax3.YDirection()) < 0 ) ax3.YReverse();
trsf.SetTransformation(ax3);
trsf.Invert();
const double scale1 = l->hasScale() ? l->Scale() : 1.0f;
const double scale2 = l->hasScale2() ? l->Scale2() : scale1;
const double scale3 = l->hasScale3() ? l->Scale3() : scale1;
gtrsf = gp_GTrsf();
gtrsf.SetValue(1,1,scale1);
gtrsf.SetValue(2,2,scale2);
gtrsf.SetValue(3,3,scale3);
gtrsf.PreMultiply(trsf);
CACHE(IfcCartesianTransformationOperator3DnonUniform,l,gtrsf)
return true;
}
// Called when the user moves the mouse in the main window
// while a button is held down
void Cmesh_mesh_collisionsApp::scroll(CPoint p, int left_button) {
// If the user hasn't clicked on any objects, we don't
// have to move or rotate anyone
if (selected_object == 0) return;
cGenericObject* object_to_move = selected_object;
// If the left button is being held down, rotate the
// selected object
if (left_button) {
cVector3d axis1(-1,0,0);
object_to_move->rotate(axis1,-1.0*(float)p.y / 50.0);
cVector3d axis2(0,1,0);
object_to_move->rotate(axis2,(float)p.x / 50.0);
}
// If the left button is being held down, move the
// selected object
else {
object_to_move->translate((float)p.x / 100.0, 0, 0);
object_to_move->translate(0, -1.0*(float)p.y / 100.0, 0);
}
// Let the object re-compute his global position data
object->computeGlobalPositions();
object->computeBoundaryBox(true);
}
ImplicitFuncCSG::ImplicitFuncCSG(const BBox<scalar,3>& bb,OP_TYPE op):_alpha(0.8f),_op(op)
{
BBox<scalar,2> bb2(bb._minC.block(0,0,2,1),bb._maxC.block(0,0,2,1));
boost::shared_ptr<ImplicitFuncCSG> axis01(new ImplicitFuncCSG(bb2,op));
boost::shared_ptr<ImplicitFuncCSG> axis2(new ImplicitFuncCSG(op));
if(op == INTERSECT) {
axis2->_a.reset(new ImplicitFuncPlane(Vec3(0.0f,0.0f,bb._minC.z()),Vec3(0.0f,0.0f,-1.0f)));
axis2->_b.reset(new ImplicitFuncPlane(Vec3(0.0f,0.0f,bb._maxC.z()),Vec3(0.0f,0.0f, 1.0f)));
} else {
axis2->_a.reset(new ImplicitFuncPlane(Vec3(0.0f,0.0f,bb._minC.z()),Vec3(0.0f,0.0f, 1.0f)));
axis2->_b.reset(new ImplicitFuncPlane(Vec3(0.0f,0.0f,bb._maxC.z()),Vec3(0.0f,0.0f,-1.0f)));
}
_a=axis01;
_b=axis2;
}
// Called when the user moves the mouse in the main window
// while a button is held down
void Crecord_playerApp::scroll(CPoint p, int left_button) {
// For now, we're going to disable mouse interaction in
// this example to simplify the interaction between
// the haptic device and the record player...
return;
// If the user hasn't clicked on any objects, we don't
// have to move or rotate anyone
//if (selected_object == 0) return;
cGenericObject* object_to_move = selected_object;
// If the left button is being held down, rotate the
// selected object
if (left_button) {
m_recordMesh->translate(cMul(-0.04, m_recordMesh->getRot().getCol2()));
cVector3d axis1(-1,0,0);
object->rotate(axis1,-1.0*(float)p.y / 50.0);
m_recordMesh->rotate(axis1,-1.0*(float)p.y / 50.0);
cVector3d axis2(0,1,0);
object->rotate(axis2,(float)p.x / 50.0);
m_recordMesh->rotate(axis2,(float)p.x / 50.0);
m_recordMesh->translate(cMul(0.04, m_recordMesh->getRot().getCol2()));
}
// If the left button is being held down, move the
// selected object
else {
object->translate(0, 0, -1.0*(float)p.y / 100.0);
m_recordMesh->translate(0, 0, -1.0*(float)p.y / 100.0);
object->translate(0, 1.0*(float)p.x / 100.0, 0);
m_recordMesh->translate(0, 1.0*(float)p.x / 100.0, 0);
}
// Let the object re-compute his global position data
object->computeGlobalPositions();
m_recordMesh->computeGlobalPositions();
object->computeBoundaryBox(true);
}
void mouseMove(int x, int y)
{
if (buttonDown == -1) return;
int dx = x - lastX;
int dy = y - lastY;
lastX = x;
lastY = y;
if (buttonDown == GLUT_LEFT_BUTTON)
{
// rotate the model
// These vectors come from the (unusual) definition of the CHAI
// camera's rotation matrix:
//
// column 0: look
// column 2: up
// column 1: look x up
// Rotation around the horizontal camera axis
cVector3d axis1(0,1,0);
camera->getRot().mul(axis1);
object->rotate(axis1,1.0*(float)dy / 50.0);
// Rotation around the vertical camera axis
cVector3d axis2(0,0,1);
camera->getRot().mul(axis2);
object->rotate(axis2,(float)dx / 50.0);
object->computeGlobalPositions(true);
}
else
{
// move the model
cVector3d translation_vector =
(-1.0*(float)dy / 100.0) * camera->getUpVector() +
( 1.0*(float)dx / 100.0) * camera->getRightVector();
object->translate(translation_vector);
object->computeGlobalPositions(true);
}
}
bool IfcGeom::convert(const Ifc2x3::IfcCartesianTransformationOperator3D::ptr l, gp_Trsf& trsf) {
IN_CACHE(IfcCartesianTransformationOperator3D,l,gp_Trsf,trsf)
gp_Pnt origin;
IfcGeom::convert(l->LocalOrigin(),origin);
gp_Dir axis1 (1.,0.,0.);
gp_Dir axis2 (0.,1.,0.);
gp_Dir axis3;
if ( l->hasAxis1() ) IfcGeom::convert(l->Axis1(),axis1);
if ( l->hasAxis2() ) IfcGeom::convert(l->Axis2(),axis2);
if ( l->hasAxis3() ) IfcGeom::convert(l->Axis3(),axis3);
else axis3 = axis1.Crossed(axis2);
gp_Ax3 ax3 (origin,axis3,axis1);
if ( axis2.Dot(ax3.YDirection()) < 0 ) ax3.YReverse();
trsf.SetTransformation(ax3);
trsf.Invert();
if ( l->hasScale() ) trsf.SetScaleFactor(l->Scale());
CACHE(IfcCartesianTransformationOperator3D,l,trsf)
return true;
}
shared_ptr<coil::RenderObj>
LOscillatingPlate::getCoilRenderObj() const
{
const double lengthRescale = 1 / Sim->primaryCellSize.maxElement();
if (!_renderObj)
{
Vector axis3 = nhat / nhat.nrm();
Vector axis2(0,0,1);
for (size_t i(0); i < NDIM; ++i)
{
Vector tryaxis = Vector(0,0,0);
tryaxis[i] = 1;
Vector tryaxis2 = axis3 ^ tryaxis;
if (tryaxis2.nrm() != 0) { axis2 = tryaxis2 / tryaxis2.nrm(); break; }
}
Vector axis1 = axis2 ^ axis3;
std::ostringstream os;
os << axis3[0] << ", "
<< axis3[1] << ", "
<< axis3[2] << ", 0";
axis1 *= Sim->primaryCellSize[1] * lengthRescale / axis1.nrm();
axis2 *= Sim->primaryCellSize[2] * lengthRescale / axis2.nrm();
_renderObj.reset(new coil::RFunction(10,
rw0 - 0.5 * (axis1 + axis2),
axis1, axis2, axis3,
0, 0, 1, 1, true, false,
"Oscillating wall",
"f = A;",
"normal = -(float4)(" + os.str() + ");"
));
}
return std::tr1::static_pointer_cast<coil::RenderObj>(_renderObj);
}
void Transformable::setTransformOrientation(Vector3D axis,float angle)
{
axis.normalize();
float c = std::cos(angle);
float s = std::sin(angle);
float a = 1 - c;
Vector3D axis2(axis.x * axis.x,axis.y * axis.y,axis.z * axis.z);
local[0] = axis2.x + (1 - axis2.x) * c;
local[1] = axis.x * axis.y * a + axis.z * s;
local[2] = axis.x * axis.z * a - axis.y * s;
local[4] = axis.x * axis.y * a - axis.z * s;
local[5] = axis2.y + (1 - axis2.y) * c;
local[6] = axis.y * axis.z * a + axis.x * s;
local[8] = axis.x * axis.z * a + axis.y * s;
local[9] = axis.y * axis.z * a - axis.x * s;
local[10] = axis2.z + (1 - axis2.z) * c;
localIdentity = false;
notifyForUpdate();
}
//----------------------------------------------------------------------------
void RoughPlaneSolidBox::MoveBox ()
{
float x = (float)mModule.GetX();
float w = (float)mModule.GetW();
float xExt = (float)mModule.XLocExt;
float yExt = (float)mModule.YLocExt;
float zExt = (float)mModule.ZLocExt;
float sinPhi = (float)mModule.SinAngle;
float cosPhi = (float)mModule.CosAngle;
float theta = (float)mModule.GetTheta();
float sinTheta = Mathf::Sin(theta);
float cosTheta = Mathf::Cos(theta);
// Compute the box center.
APoint center(x, w*cosPhi - zExt*sinPhi, w*sinPhi + zExt*cosPhi);
// Compute the box orientation.
AVector axis0(cosTheta, -sinTheta*cosPhi, -sinTheta*sinPhi);
AVector axis1(sinTheta, +cosTheta*cosPhi, +cosTheta*sinPhi);
AVector axis2(0.0f, -sinPhi, cosPhi);
// Keep the box from sliding below the ground.
float zRadius =
xExt*Mathf::FAbs(axis0.Z()) +
yExt*Mathf::FAbs(axis1.Z()) +
zExt*Mathf::FAbs(axis2.Z());
if (center.Z() >= zRadius)
{
// Update the box.
mBox->LocalTransform.SetTranslate(center);
mBox->LocalTransform.SetRotate(HMatrix(axis0, axis1, axis2,
APoint::ORIGIN, true));
mBox->Update();
}
else
{
mDoUpdate = false;
}
}
/**
* Load a single entry into a workspace
* @param root :: The opened root node
* @param entry_name :: The entry name
* @param progressStart :: The percentage value to start the progress reporting for this entry
* @param progressRange :: The percentage range that the progress reporting should cover
* @returns A 2D workspace containing the loaded data
*/
API::Workspace_sptr LoadNexusProcessed::loadEntry(NXRoot & root, const std::string & entry_name,
const double& progressStart, const double& progressRange)
{
progress(progressStart,"Opening entry " + entry_name + "...");
NXEntry mtd_entry = root.openEntry(entry_name);
if (mtd_entry.containsGroup("table_workspace"))
{
return loadTableEntry(mtd_entry);
}
bool isEvent = false;
std::string group_name = "workspace";
if (mtd_entry.containsGroup("event_workspace"))
{
isEvent = true;
group_name = "event_workspace";
}
// Get workspace characteristics
NXData wksp_cls = mtd_entry.openNXData(group_name);
// Axis information
// "X" axis
NXDouble xbins = wksp_cls.openNXDouble("axis1");
xbins.load();
std::string unit1 = xbins.attributes("units");
// Non-uniform x bins get saved as a 2D 'axis1' dataset
int xlength(-1);
if( xbins.rank() == 2 )
{
xlength = xbins.dim1();
m_shared_bins = false;
}
else if( xbins.rank() == 1 )
{
xlength = xbins.dim0();
m_shared_bins = true;
xbins.load();
m_xbins.access().assign(xbins(), xbins() + xlength);
}
else
{
throw std::runtime_error("Unknown axis1 dimension encountered.");
}
// MatrixWorkspace axis 1
NXDouble axis2 = wksp_cls.openNXDouble("axis2");
std::string unit2 = axis2.attributes("units");
// The workspace being worked on
API::MatrixWorkspace_sptr local_workspace;
size_t nspectra;
int64_t nchannels;
// -------- Process as event ? --------------------
if (isEvent)
{
local_workspace = loadEventEntry(wksp_cls, xbins, progressStart, progressRange);
nspectra = local_workspace->getNumberHistograms();
nchannels = local_workspace->blocksize();
}
else
{
NXDataSetTyped<double> data = wksp_cls.openDoubleData();
nspectra = data.dim0();
nchannels = data.dim1();
//// validate the optional spectrum parameters, if set
checkOptionalProperties(nspectra);
// Actual number of spectra in output workspace (if only a range was going to be loaded)
int total_specs=calculateWorkspacesize(nspectra);
//// Create the 2D workspace for the output
local_workspace = boost::dynamic_pointer_cast<API::MatrixWorkspace>
(WorkspaceFactory::Instance().create("Workspace2D", total_specs, xlength, nchannels));
try
{
local_workspace->setTitle(mtd_entry.getString("title"));
}
catch (std::runtime_error&)
{
g_log.debug() << "No title was found in the input file, " << getPropertyValue("Filename") << std::endl;
}
// Set the YUnit label
local_workspace->setYUnit(data.attributes("units"));
std::string unitLabel = data.attributes("unit_label");
if (unitLabel.empty()) unitLabel = data.attributes("units");
local_workspace->setYUnitLabel(unitLabel);
readBinMasking(wksp_cls, local_workspace);
NXDataSetTyped<double> errors = wksp_cls.openNXDouble("errors");
int64_t blocksize(8);
//const int fullblocks = nspectra / blocksize;
//size of the workspace
int64_t fullblocks = total_specs / blocksize;
int64_t read_stop = (fullblocks * blocksize);
const double progressBegin = progressStart+0.25*progressRange;
const double progressScaler = 0.75*progressRange;
int64_t hist_index = 0;
int64_t wsIndex=0;
if( m_shared_bins )
{
//if spectrum min,max,list properties are set
if(m_interval||m_list)
{
//if spectrum max,min properties are set read the data as a block(multiple of 8) and
//then read the remaining data as finalblock
if(m_interval)
{
//specs at the min-max interval
int interval_specs=static_cast<int>(m_spec_max-m_spec_min);
fullblocks=(interval_specs)/blocksize;
read_stop = (fullblocks * blocksize)+m_spec_min-1;
if(interval_specs<blocksize)
{
blocksize=total_specs;
read_stop=m_spec_max-1;
}
hist_index=m_spec_min-1;
for( ; hist_index < read_stop; )
{
progress(progressBegin+progressScaler*static_cast<double>(hist_index)/static_cast<double>(read_stop),"Reading workspace data...");
loadBlock(data, errors, blocksize, nchannels, hist_index,wsIndex, local_workspace);
}
int64_t finalblock = m_spec_max-1 - read_stop;
if( finalblock > 0 )
{
loadBlock(data, errors, finalblock, nchannels, hist_index,wsIndex,local_workspace);
}
}
// if spectrum list property is set read each spectrum separately by setting blocksize=1
if(m_list)
{
std::vector<int64_t>::iterator itr=m_spec_list.begin();
for(;itr!=m_spec_list.end();++itr)
{
int64_t specIndex=(*itr)-1;
progress(progressBegin+progressScaler*static_cast<double>(specIndex)/static_cast<double>(m_spec_list.size()),"Reading workspace data...");
loadBlock(data, errors, static_cast<int64_t>(1), nchannels, specIndex,wsIndex, local_workspace);
}
}
}
else
{
for( ; hist_index < read_stop; )
{
progress(progressBegin+progressScaler*static_cast<double>(hist_index)/static_cast<double>(read_stop),"Reading workspace data...");
loadBlock(data, errors, blocksize, nchannels, hist_index,wsIndex, local_workspace);
}
int64_t finalblock = total_specs - read_stop;
if( finalblock > 0 )
{
loadBlock(data, errors, finalblock, nchannels, hist_index,wsIndex,local_workspace);
}
}
}
else
{
if(m_interval||m_list)
{
if(m_interval)
{
int64_t interval_specs=m_spec_max-m_spec_min;
fullblocks=(interval_specs)/blocksize;
read_stop = (fullblocks * blocksize)+m_spec_min-1;
if(interval_specs<blocksize)
{
blocksize=interval_specs;
read_stop=m_spec_max-1;
}
hist_index=m_spec_min-1;
for( ; hist_index < read_stop; )
{
progress(progressBegin+progressScaler*static_cast<double>(hist_index)/static_cast<double>(read_stop),"Reading workspace data...");
loadBlock(data, errors, xbins, blocksize, nchannels, hist_index,wsIndex,local_workspace);
}
int64_t finalblock = m_spec_max-1 - read_stop;
if( finalblock > 0 )
{
loadBlock(data, errors, xbins, finalblock, nchannels, hist_index,wsIndex, local_workspace);
}
}
//
if(m_list)
{
std::vector<int64_t>::iterator itr=m_spec_list.begin();
for(;itr!=m_spec_list.end();++itr)
{
int64_t specIndex=(*itr)-1;
progress(progressBegin+progressScaler*static_cast<double>(specIndex)/static_cast<double>(read_stop),"Reading workspace data...");
loadBlock(data, errors, xbins, 1, nchannels, specIndex,wsIndex,local_workspace);
}
}
}
else
{
for( ; hist_index < read_stop; )
{
progress(progressBegin+progressScaler*static_cast<double>(hist_index)/static_cast<double>(read_stop),"Reading workspace data...");
loadBlock(data, errors, xbins, blocksize, nchannels, hist_index,wsIndex,local_workspace);
}
int64_t finalblock = total_specs - read_stop;
if( finalblock > 0 )
{
loadBlock(data, errors, xbins, finalblock, nchannels, hist_index,wsIndex, local_workspace);
}
}
}
} //end of NOT an event -------------------------------
//Units
try
{
local_workspace->getAxis(0)->unit() = UnitFactory::Instance().create(unit1);
//If this doesn't throw then it is a numeric access so grab the data so we can set it later
axis2.load();
m_axis1vals = MantidVec(axis2(), axis2() + axis2.dim0());
}
catch( std::runtime_error & )
{
g_log.information() << "Axis 0 set to unitless quantity \"" << unit1 << "\"\n";
}
// Setting a unit onto a SpectraAxis makes no sense.
if ( unit2 == "TextAxis" )
{
Mantid::API::TextAxis* newAxis = new Mantid::API::TextAxis(nspectra);
local_workspace->replaceAxis(1, newAxis);
}
else if ( unit2 != "spectraNumber" )
{
try
{
Mantid::API::NumericAxis* newAxis = new Mantid::API::NumericAxis(nspectra);
local_workspace->replaceAxis(1, newAxis);
newAxis->unit() = UnitFactory::Instance().create(unit2);
}
catch( std::runtime_error & )
{
g_log.information() << "Axis 1 set to unitless quantity \"" << unit2 << "\"\n";
}
}
//Are we a distribution
std::string dist = xbins.attributes("distribution");
if( dist == "1" )
{
local_workspace->isDistribution(true);
}
else
{
local_workspace->isDistribution(false);
}
//Get information from all but data group
std::string parameterStr;
progress(progressStart+0.05*progressRange,"Reading the sample details...");
// Hop to the right point
cppFile->openPath(mtd_entry.path());
try
{
// This loads logs, sample, and instrument.
local_workspace->loadExperimentInfoNexus(cppFile, parameterStr);
}
catch (std::exception & e)
{
g_log.information("Error loading Instrument section of nxs file");
g_log.information(e.what());
}
// Now assign the spectra-detector map
readInstrumentGroup(mtd_entry, local_workspace);
// Parameter map parsing
progress(progressStart+0.11*progressRange,"Reading the parameter maps...");
local_workspace->readParameterMap(parameterStr);
if ( ! local_workspace->getAxis(1)->isSpectra() )
{ // If not a spectra axis, load the axis data into the workspace. (MW 25/11/10)
loadNonSpectraAxis(local_workspace, wksp_cls);
}
progress(progressStart+0.15*progressRange,"Reading the workspace history...");
try
{
readAlgorithmHistory(mtd_entry, local_workspace);
}
catch (std::out_of_range&)
{
g_log.warning() << "Error in the workspaces algorithm list, its processing history is incomplete\n";
}
progress(progressStart+0.2*progressRange,"Reading the workspace history...");
return boost::static_pointer_cast<API::Workspace>(local_workspace);
}
static void __DecomposeMatrices(float *matrices, size_t count,
std::vector< shared_ptr <GLTFBufferView> > &TRSBufferViews) {
size_t translationBufferSize = sizeof(float) * 3 * count;
size_t rotationBufferSize = sizeof(float) * 4 * count;
size_t scaleBufferSize = sizeof(float) * 3 * count;
float *translationData = (float*)malloc(translationBufferSize);
float *rotationData = (float*)malloc(rotationBufferSize);
float *scaleData = (float*)malloc(scaleBufferSize);
shared_ptr <GLTF::GLTFBufferView> translationBufferView = createBufferViewWithAllocatedBuffer(translationData, 0, translationBufferSize, true);
shared_ptr <GLTF::GLTFBufferView> rotationBufferView = createBufferViewWithAllocatedBuffer(rotationData, 0, rotationBufferSize, true);
shared_ptr <GLTF::GLTFBufferView> scaleBufferView = createBufferViewWithAllocatedBuffer(scaleData, 0, scaleBufferSize, true);
float *previousRotation = 0;
for (size_t i = 0 ; i < count ; i++) {
float *m = matrices;
COLLADABU::Math::Matrix4 mat;
mat.setAllElements(m[0], m[1], m[2], m[3],
m[4], m[5], m[6], m[7],
m[8], m[9], m[10], m[11],
m[12], m[13], m[14], m[15] );
decomposeMatrix(mat, translationData, rotationData, scaleData);
//make sure we export the short path from orientations
if (0 != previousRotation) {
COLLADABU::Math::Vector3 axis1(previousRotation[0], previousRotation[1], previousRotation[2]);
COLLADABU::Math::Vector3 axis2(rotationData[0], rotationData[1], rotationData[2]);
COLLADABU::Math::Quaternion key1;
COLLADABU::Math::Quaternion key2;
key1.fromAngleAxis(previousRotation[3], axis1);
key2.fromAngleAxis(rotationData[3], axis2);
COLLADABU::Math::Real cosHalfTheta = key1.dot(key2);
if (cosHalfTheta < 0) {
key2.x = -key2.x;
key2.y = -key2.y;
key2.z = -key2.z;
key2.w = -key2.w;
COLLADABU::Math::Real angle;
key2.toAngleAxis(angle, axis2);
rotationData[3] = (float)angle;
rotationData[0] = (float)axis2.x;
rotationData[1] = (float)axis2.y;
rotationData[2] = (float)axis2.z;
key2.fromAngleAxis(rotationData[3], axis2);
//FIXME: this needs to be refined, we ensure continuity here, but assume in clockwise order
cosHalfTheta = key1.dot(key2);
if (cosHalfTheta < 0) {
rotationData[3] += (float)(2. * 3.14159265359);
key2.fromAngleAxis(rotationData[3], axis2);
}
}
}
previousRotation = rotationData;
translationData += 3;
rotationData += 4;
scaleData += 3;
matrices += 16;
}
/*
rotationData = (float*)rotationBufferView->getBufferDataByApplyingOffset();
for (size_t i = 0 ; i < count ; i++) {
printf("rotation at:%d %f %f %f %f\n", i,
rotationData[0],rotationData[1],rotationData[2],rotationData[3]);
rotationData += 4;
}
*/
TRSBufferViews.push_back(translationBufferView);
TRSBufferViews.push_back(rotationBufferView);
TRSBufferViews.push_back(scaleBufferView);
}
// ################################################
double NeuralNetwork::LearnGivenData(int MiniBatchSize, int epochs, int TrainingsSize, bool TestOnTrainingsData, bool TestOnTestingData, std::string SavePath){
TCanvas c1("Accuracy", "Accuracy", 600, 500);
TCanvas c2("Cost", "Cost", 600, 500);
TCanvas c3("summary", "summary", 1200, 500);
c3.SetGrid();
c3.Divide(2);
c1.cd();
TH1F axis("axis", "Accuracy of Neural Network;epochs", epochs, 0.5, epochs+0.5);
TH1F acc_test ("acc_test" , "Classification accuracy", epochs, 0.5, epochs+0.5);
TH1F acc_train("acc_train", "Classification accuracy", epochs, 0.5, epochs+0.5);
TH1F axis2("axis2", "Cost Function of Neural Network;epochs", epochs, 0.5, epochs+0.5);
TH1F cost_test ("cost_test" , "Cost Function", epochs, 0.5, epochs+0.5);
TH1F cost_train("cost_train", "Cost Function", epochs, 0.5, epochs+0.5);
if (TrainingsSize == -1) TrainingsSize = fTrainingsSize;
for (int k = 0; k < epochs; ++k){
// if (k % 5 == 0) std::cout << "Epoch nr. " << k << " started!" << std::endl;
for (int i = 0; i < TrainingsSize / MiniBatchSize; ++i){
for (int j = 0; j < MiniBatchSize; ++j){
SetInputVectorInMiniBatch(fTrainingsdata[i*10 + j], fTrainingslabel[i*10 + j]);
}
LearnMiniBatches();
}
if (TestOnTestingData){
double correct = 0;
double cost = 0;
for (int i = 0; i < fTestingSize; ++i){
if (Evaluate(fTestingdata[i], fTestinglabel[i]) == true) correct++;
cost += EvaluateCost(fTestingdata[i], fTestinglabel[i]);
}
// std::cout << "Accuracy of test data: " << correct / fTestingSize << " %"<< std::endl;
acc_test.SetBinContent(k+1, correct / fTestingSize);
cost_test.SetBinContent(k+1, cost / fTestingSize);
}
if (TestOnTrainingsData){
double correct = 0;
double cost = 0;
for (int i = 0; i < TrainingsSize; ++i){
if (Evaluate(fTrainingsdata[i], fTrainingslabel[i]) == true) correct++;
cost += EvaluateCost(fTrainingsdata[i], fTrainingslabel[i]);
}
// std::cout << "Accuracy of trainings data: " << correct / TrainingsSize << " %" << std::endl;
acc_train.SetBinContent(k+1, correct / TrainingsSize);
cost_train.SetBinContent(k+1, cost / fTrainingsSize);
}
}
axis.SetStats(false);
axis.GetYaxis()->SetNdivisions(524);
axis.SetAxisRange(0.5, 1.1, "Y");
axis.Draw("");
double markersize = 1;
if (epochs > 100) {
markersize = 0.5;
c1.SetLogx();
}
acc_test.SetStats(false);
acc_test.SetMarkerStyle(20);
acc_test.SetMarkerColor(kOrange-3);
acc_test.SetMarkerSize(1.);
acc_test.Draw("p same");
acc_train.SetStats(false);
acc_train.SetMarkerStyle(20);
acc_train.SetMarkerColor(kAzure-2);
acc_train.SetMarkerSize(1.);
acc_train.Draw("p same");
// c1.SetLogx();
c1.SetGrid();
// Calculate maxima
double max_trainingsdata = acc_train.GetMaximum();
double max_testdata = acc_test.GetMaximum();
TLegend leg(0.4,0.1,0.9,0.3);
leg.SetHeader("Accuracy of");
leg.AddEntry(&acc_test, Form("Test data (Maximum: %4.3f)", max_testdata), "p");
leg.AddEntry(&acc_train, Form("Trainings data (Maximum: %4.3f)", max_trainingsdata), "p");
leg.Draw("same");
c1.SaveAs(Form("%saccuracy.pdf", SavePath.c_str()));
c2.cd();
axis2.SetStats(false);
axis2.GetYaxis()->SetNdivisions(524);
axis2.SetAxisRange(cost_train.GetMinimum()*0.9, cost_test.GetMaximum()*1.1, "Y");
axis2.Draw("");
if (epochs > 100) {
markersize = 0.5;
c2.SetLogx();
}
cost_train.SetStats(false);
cost_train.SetMarkerStyle(20);
cost_train.SetMarkerColor(kAzure-2);
cost_train.SetMarkerSize(1.);
cost_train.Draw("p same");
cost_test.SetStats(false);
cost_test.SetMarkerStyle(20);
cost_test.SetMarkerColor(kOrange-3);
cost_test.SetMarkerSize(1.);
cost_test.Draw("p same");
// c1.SetLogx();
c2.SetGrid();
// Calculate maxima
double min_trainingsdata = cost_train.GetMinimum();
double min_testdata = cost_test.GetMinimum();
TLegend leg2(0.4,0.7,0.9,0.9);
leg2.SetHeader("Cost Function");
leg2.AddEntry(&acc_test, Form("Test data (Minimum: %4.3f)", min_testdata), "p");
leg2.AddEntry(&acc_train, Form("Trainings data (Minimum: %4.3f)", min_trainingsdata), "p");
leg2.Draw("same");
c2.SaveAs(Form("%scost.pdf", SavePath.c_str()));
c3.cd(1);
axis.Draw("");
acc_test.Draw("p same");
acc_train.Draw("p same");
leg.Draw("same");
c3.cd(2);
axis2.Draw("");
cost_test.Draw("p same");
cost_train.Draw("p same");
leg2.Draw("same");
c3.SaveAs(Form("%ssummary.pdf", SavePath.c_str()));
// std::cout << "Learning completed" << std::endl;
return max_testdata;
}
//---------------------------------------------------------
void Poly3D::SortPoints(const DVec& cent)
//---------------------------------------------------------
{
// Sort the points by angle from cent which is
// a point in the plane of the polygon. This
// is done in a counter-clockwise direction.
// If less than 2 points, no need to sort
if (m_N < 2) {
return;
}
// create local cartesian axis
DVec ref1,ref2,nor,axis1,axis2,vI,vJ,tmp;
// wrap DM with DMat for utility routines
int Nr=m_xyz.num_rows(), Nc=m_xyz.num_cols();
DMat t_xyz; t_xyz.borrow(Nr, Nc, m_xyz.data());
t_xyz.get_col(1) - cent; ref1 /= ref1.norm();
// get two lines in the plane
ref1 = t_xyz.get_col(1) - cent; ref1 /= ref1.norm();
ref2 = t_xyz.get_col(2) - cent; ref2 /= ref2.norm();
// normal to the plane (norm = ref1 X ref2)
nor(1) = ref1(2)*ref2(3) - ref1(3)*ref2(2);
nor(2) = ref1(3)*ref2(1) - ref1(1)*ref2(3);
nor(3) = ref1(1)*ref2(2) - ref1(2)*ref2(1);
nor /= nor.norm();
// axis definition
axis1 = ref1;
// axis2 = norm x axis1
axis2(1) = nor(2)*axis1(3) - nor(3)*axis1(2);
axis2(2) = nor(3)*axis1(1) - nor(1)*axis1(3);
axis2(3) = nor(1)*axis1(2) - nor(2)*axis1(1);
double costhetaI,sinthetaI, costhetaJ,sinthetaJ, thetaI,thetaJ;
for (int i=1; i<=m_N; ++i) {
for (int j=(i+1); j<=m_N; ++j) {
vI = t_xyz.get_col(i) - cent;
vJ = t_xyz.get_col(j) - cent;
costhetaI = vI.inner(axis1);
sinthetaI = vI.inner(axis2);
costhetaJ = vJ.inner(axis1);
sinthetaJ = vJ.inner(axis2);
thetaI = atan2(sinthetaI, costhetaI);
thetaJ = atan2(sinthetaJ, costhetaJ);
// sort minimum angle difference first
if (thetaJ < thetaI)
{
// swap I and J
//t_xyz(All, [i j]) = t_xyz(All, [j i]);
tmp = t_xyz.get_col(i); // copy column i
t_xyz.set_col(i, t_xyz.get_col(j)); // overwrite col i
t_xyz.set_col(j, tmp); // overwrite col j
}
}
}
}
SOrientedBoundingBox *
SOrientedBoundingBox::buildOBB(std::vector<SPoint3> &vertices)
{
#if defined(HAVE_MESH)
int num_vertices = vertices.size();
// First organize the data
std::set<SPoint3> unique;
unique.insert(vertices.begin(), vertices.end());
num_vertices = unique.size();
fullMatrix<double> data(3, num_vertices);
fullVector<double> mean(3);
fullVector<double> vmins(3);
fullVector<double> vmaxs(3);
mean.setAll(0);
vmins.setAll(DBL_MAX);
vmaxs.setAll(-DBL_MAX);
size_t idx = 0;
for(std::set<SPoint3>::iterator uIter = unique.begin(); uIter != unique.end();
++uIter) {
const SPoint3 &pp = *uIter;
for(int d = 0; d < 3; d++) {
data(d, idx) = pp[d];
vmins(d) = std::min(vmins(d), pp[d]);
vmaxs(d) = std::max(vmaxs(d), pp[d]);
mean(d) += pp[d];
}
idx++;
}
for(int i = 0; i < 3; i++) { mean(i) /= num_vertices; }
// Get the deviation from the mean
fullMatrix<double> B(3, num_vertices);
for(int i = 0; i < 3; i++) {
for(int j = 0; j < num_vertices; j++) { B(i, j) = data(i, j) - mean(i); }
}
// Compute the covariance matrix
fullMatrix<double> covariance(3, 3);
B.mult(B.transpose(), covariance);
covariance.scale(1. / (num_vertices - 1));
/*
Msg::Debug("Covariance matrix");
Msg::Debug("%f %f %f", covariance(0,0),covariance(0,1),covariance(0,2) );
Msg::Debug("%f %f %f", covariance(1,0),covariance(1,1),covariance(1,2) );
Msg::Debug("%f %f %f", covariance(2,0),covariance(2,1),covariance(2,2) );
*/
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
if(std::abs(covariance(i, j)) < 10e-16) covariance(i, j) = 0;
}
}
fullMatrix<double> left_eigv(3, 3);
fullMatrix<double> right_eigv(3, 3);
fullVector<double> real_eig(3);
fullVector<double> img_eig(3);
covariance.eig(real_eig, img_eig, left_eigv, right_eigv, true);
// Now, project the data in the new basis.
fullMatrix<double> projected(3, num_vertices);
left_eigv.transpose().mult(data, projected);
// Get the size of the box in the new direction
fullVector<double> mins(3);
fullVector<double> maxs(3);
for(int i = 0; i < 3; i++) {
mins(i) = DBL_MAX;
maxs(i) = -DBL_MAX;
for(int j = 0; j < num_vertices; j++) {
maxs(i) = std::max(maxs(i), projected(i, j));
mins(i) = std::min(mins(i), projected(i, j));
}
}
// double means[3];
double sizes[3];
// Note: the size is computed in the box's coordinates!
for(int i = 0; i < 3; i++) {
sizes[i] = maxs(i) - mins(i);
// means[i] = (maxs(i) - mins(i)) / 2.;
}
if(sizes[0] == 0 && sizes[1] == 0) {
// Entity is a straight line...
SVector3 center;
SVector3 Axis1;
SVector3 Axis2;
SVector3 Axis3;
Axis1[0] = left_eigv(0, 0);
Axis1[1] = left_eigv(1, 0);
Axis1[2] = left_eigv(2, 0);
Axis2[0] = left_eigv(0, 1);
Axis2[1] = left_eigv(1, 1);
Axis2[2] = left_eigv(2, 1);
Axis3[0] = left_eigv(0, 2);
Axis3[1] = left_eigv(1, 2);
Axis3[2] = left_eigv(2, 2);
center[0] = (vmaxs(0) + vmins(0)) / 2.0;
center[1] = (vmaxs(1) + vmins(1)) / 2.0;
center[2] = (vmaxs(2) + vmins(2)) / 2.0;
return new SOrientedBoundingBox(center, sizes[0], sizes[1], sizes[2], Axis1,
Axis2, Axis3);
}
// We take the smallest component, then project the data on the plane defined
// by the other twos
int smallest_comp = 0;
if(sizes[0] <= sizes[1] && sizes[0] <= sizes[2])
smallest_comp = 0;
else if(sizes[1] <= sizes[0] && sizes[1] <= sizes[2])
smallest_comp = 1;
else if(sizes[2] <= sizes[0] && sizes[2] <= sizes[1])
smallest_comp = 2;
// The projection has been done circa line 161.
// We just ignore the coordinate corresponding to smallest_comp.
std::vector<SPoint2 *> points;
for(int i = 0; i < num_vertices; i++) {
SPoint2 *p = new SPoint2(projected(smallest_comp == 0 ? 1 : 0, i),
projected(smallest_comp == 2 ? 1 : 2, i));
bool keep = true;
for(std::vector<SPoint2 *>::iterator point = points.begin();
point != points.end(); point++) {
if(std::abs((*p)[0] - (**point)[0]) < 10e-10 &&
std::abs((*p)[1] - (**point)[1]) < 10e-10) {
keep = false;
break;
}
}
if(keep) { points.push_back(p); }
else {
delete p;
}
}
// Find the convex hull from a delaunay triangulation of the points
DocRecord record(points.size());
record.numPoints = points.size();
srand((unsigned)time(0));
for(std::size_t i = 0; i < points.size(); i++) {
record.points[i].where.h =
points[i]->x() + (10e-6) * sizes[smallest_comp == 0 ? 1 : 0] *
(-0.5 + ((double)rand()) / RAND_MAX);
record.points[i].where.v =
points[i]->y() + (10e-6) * sizes[smallest_comp == 2 ? 1 : 0] *
(-0.5 + ((double)rand()) / RAND_MAX);
record.points[i].adjacent = NULL;
}
try {
record.MakeMeshWithPoints();
} catch(const char *err) {
Msg::Error("%s", err);
}
std::vector<Segment> convex_hull;
for(int i = 0; i < record.numTriangles; i++) {
Segment segs[3];
segs[0].from = record.triangles[i].a;
segs[0].to = record.triangles[i].b;
segs[1].from = record.triangles[i].b;
segs[1].to = record.triangles[i].c;
segs[2].from = record.triangles[i].c;
segs[2].to = record.triangles[i].a;
for(int j = 0; j < 3; j++) {
bool okay = true;
for(std::vector<Segment>::iterator seg = convex_hull.begin();
seg != convex_hull.end(); seg++) {
if(((*seg).from == segs[j].from && (*seg).from == segs[j].to)
// FIXME:
// || ((*seg).from == segs[j].to && (*seg).from == segs[j].from)
) {
convex_hull.erase(seg);
okay = false;
break;
}
}
if(okay) { convex_hull.push_back(segs[j]); }
}
}
// Now, examinate all the directions given by the edges of the convex hull
// to find the one that lets us build the least-area bounding rectangle for
// then points.
fullVector<double> axis(2);
axis(0) = 1;
axis(1) = 0;
fullVector<double> axis2(2);
axis2(0) = 0;
axis2(1) = 1;
SOrientedBoundingRectangle least_rectangle;
least_rectangle.center[0] = 0.0;
least_rectangle.center[1] = 0.0;
least_rectangle.size[0] = -1.0;
least_rectangle.size[1] = 1.0;
fullVector<double> segment(2);
fullMatrix<double> rotation(2, 2);
for(std::vector<Segment>::iterator seg = convex_hull.begin();
seg != convex_hull.end(); seg++) {
// segment(0) = record.points[(*seg).from].where.h -
// record.points[(*seg).to].where.h; segment(1) =
// record.points[(*seg).from].where.v - record.points[(*seg).to].where.v;
segment(0) = points[(*seg).from]->x() - points[(*seg).to]->x();
segment(1) = points[(*seg).from]->y() - points[(*seg).to]->y();
segment.scale(1.0 / segment.norm());
double cosine = axis(0) * segment(0) + segment(1) * axis(1);
double sine = axis(1) * segment(0) - segment(1) * axis(0);
// double sine = axis(0)*segment(1) - segment(0)*axis(1);
rotation(0, 0) = cosine;
rotation(0, 1) = sine;
rotation(1, 0) = -sine;
rotation(1, 1) = cosine;
// TODO C++11 std::numeric_limits<double>
double max_x = -DBL_MAX;
double min_x = DBL_MAX;
double max_y = -DBL_MAX;
double min_y = DBL_MAX;
for(int i = 0; i < record.numPoints; i++) {
fullVector<double> pnt(2);
// pnt(0) = record.points[i].where.h;
// pnt(1) = record.points[i].where.v;
pnt(0) = points[i]->x();
pnt(1) = points[i]->y();
fullVector<double> rot_pnt(2);
rotation.mult(pnt, rot_pnt);
if(rot_pnt(0) < min_x) min_x = rot_pnt(0);
if(rot_pnt(0) > max_x) max_x = rot_pnt(0);
if(rot_pnt(1) < min_y) min_y = rot_pnt(1);
if(rot_pnt(1) > max_y) max_y = rot_pnt(1);
}
/**/
fullVector<double> center_rot(2);
fullVector<double> center_before_rot(2);
center_before_rot(0) = (max_x + min_x) / 2.0;
center_before_rot(1) = (max_y + min_y) / 2.0;
fullMatrix<double> rotation_inv(2, 2);
rotation_inv(0, 0) = cosine;
rotation_inv(0, 1) = -sine;
rotation_inv(1, 0) = sine;
rotation_inv(1, 1) = cosine;
rotation_inv.mult(center_before_rot, center_rot);
fullVector<double> axis_rot1(2);
fullVector<double> axis_rot2(2);
rotation_inv.mult(axis, axis_rot1);
rotation_inv.mult(axis2, axis_rot2);
if((least_rectangle.area() == -1) ||
(max_x - min_x) * (max_y - min_y) < least_rectangle.area()) {
least_rectangle.size[0] = max_x - min_x;
least_rectangle.size[1] = max_y - min_y;
least_rectangle.center[0] = (max_x + min_x) / 2.0;
least_rectangle.center[1] = (max_y + min_y) / 2.0;
least_rectangle.center[0] = center_rot(0);
least_rectangle.center[1] = center_rot(1);
least_rectangle.axisX[0] = axis_rot1(0);
least_rectangle.axisX[1] = axis_rot1(1);
// least_rectangle.axisX[0] = segment(0);
// least_rectangle.axisX[1] = segment(1);
least_rectangle.axisY[0] = axis_rot2(0);
least_rectangle.axisY[1] = axis_rot2(1);
}
}
// TODO C++11 std::numeric_limits<double>::min() / max()
double min_pca = DBL_MAX;
double max_pca = -DBL_MAX;
for(int i = 0; i < num_vertices; i++) {
min_pca = std::min(min_pca, projected(smallest_comp, i));
max_pca = std::max(max_pca, projected(smallest_comp, i));
}
double center_pca = (max_pca + min_pca) / 2.0;
double size_pca = (max_pca - min_pca);
double raw_data[3][5];
raw_data[0][0] = size_pca;
raw_data[1][0] = least_rectangle.size[0];
raw_data[2][0] = least_rectangle.size[1];
raw_data[0][1] = center_pca;
raw_data[1][1] = least_rectangle.center[0];
raw_data[2][1] = least_rectangle.center[1];
for(int i = 0; i < 3; i++) {
raw_data[0][2 + i] = left_eigv(i, smallest_comp);
raw_data[1][2 + i] =
least_rectangle.axisX[0] * left_eigv(i, smallest_comp == 0 ? 1 : 0) +
least_rectangle.axisX[1] * left_eigv(i, smallest_comp == 2 ? 1 : 2);
raw_data[2][2 + i] =
least_rectangle.axisY[0] * left_eigv(i, smallest_comp == 0 ? 1 : 0) +
least_rectangle.axisY[1] * left_eigv(i, smallest_comp == 2 ? 1 : 2);
}
// Msg::Info("Test 1 : %f
// %f",least_rectangle.center[0],least_rectangle.center[1]);
// Msg::Info("Test 2 : %f
// %f",least_rectangle.axisY[0],least_rectangle.axisY[1]);
int tri[3];
if(size_pca > least_rectangle.size[0]) {
// P > R0
if(size_pca > least_rectangle.size[1]) {
// P > R1
tri[0] = 0;
if(least_rectangle.size[0] > least_rectangle.size[1]) {
// R0 > R1
tri[1] = 1;
tri[2] = 2;
}
else {
// R1 > R0
tri[1] = 2;
tri[2] = 1;
}
}
else {
// P < R1
tri[0] = 2;
tri[1] = 0;
tri[2] = 1;
}
}
else { // P < R0
if(size_pca < least_rectangle.size[1]) {
// P < R1
tri[2] = 0;
if(least_rectangle.size[0] > least_rectangle.size[1]) {
tri[0] = 1;
tri[1] = 2;
}
else {
tri[0] = 2;
tri[1] = 1;
}
}
else {
tri[0] = 1;
tri[1] = 0;
tri[2] = 2;
}
}
SVector3 size;
SVector3 center;
SVector3 Axis1;
SVector3 Axis2;
SVector3 Axis3;
for(int i = 0; i < 3; i++) {
size[i] = raw_data[tri[i]][0];
center[i] = raw_data[tri[i]][1];
Axis1[i] = raw_data[tri[0]][2 + i];
Axis2[i] = raw_data[tri[1]][2 + i];
Axis3[i] = raw_data[tri[2]][2 + i];
}
SVector3 aux1;
SVector3 aux2;
SVector3 aux3;
for(int i = 0; i < 3; i++) {
aux1(i) = left_eigv(i, smallest_comp);
aux2(i) = left_eigv(i, smallest_comp == 0 ? 1 : 0);
aux3(i) = left_eigv(i, smallest_comp == 2 ? 1 : 2);
}
center = aux1 * center_pca + aux2 * least_rectangle.center[0] +
aux3 * least_rectangle.center[1];
// center[1] = -center[1];
/*
Msg::Info("Box center : %f %f %f",center[0],center[1],center[2]);
Msg::Info("Box size : %f %f %f",size[0],size[1],size[2]);
Msg::Info("Box axis 1 : %f %f %f",Axis1[0],Axis1[1],Axis1[2]);
Msg::Info("Box axis 2 : %f %f %f",Axis2[0],Axis2[1],Axis2[2]);
Msg::Info("Box axis 3 : %f %f %f",Axis3[0],Axis3[1],Axis3[2]);
Msg::Info("Volume : %f", size[0]*size[1]*size[2]);
*/
return new SOrientedBoundingBox(center, size[0], size[1], size[2], Axis1,
Axis2, Axis3);
#else
Msg::Error("SOrientedBoundingBox requires mesh module");
return 0;
#endif
}
bool ChunkyBoneGeometry::CreateJoint(ChunkyPhysics* structure, PhysicsManager* physics, unsigned physics_fps) {
bool ok = false;
if (body_data_.parent_) {
if (GetBoneType() == kBonePosition) {
// Need not do jack. It's not a physical object.
ok = true;
} else if (body_data_.joint_type_ == kJointExclude) {
ok = physics->Attach(GetBodyId(), body_data_.parent_->GetBodyId());
} else if (body_data_.joint_type_ == kJointFixed) {
ok = physics->Attach(GetBodyId(), body_data_.parent_->GetBodyId());
} else if (body_data_.joint_type_ == kJointSuspendHinge || body_data_.joint_type_ == kJointHinge2) {
// Calculate axis from given euler angles.
vec3 suspension_axis(-1, 0, 0);
vec3 hinge_axis(0, 0, 1);
quat rotator;
rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
suspension_axis = rotator*suspension_axis;
hinge_axis = rotator*hinge_axis;
joint_id_ = physics->CreateHinge2Joint(body_data_.parent_->GetBodyId(),
GetBodyId(), structure->GetTransformation(this).GetPosition(),
suspension_axis, hinge_axis);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[kParamSpringConstant],
body_data_.parameter_[kParamSpringDamping]);
physics->SetAngularMotorRoll(joint_id_, 0, 0);
physics->SetAngularMotorTurn(joint_id_, 0, 0);
ok = true;
} else if (body_data_.joint_type_ == kJointHinge) {
// Calculate axis from given euler angles.
vec3 hinge_axis(0, 0, 1);
quat hinge_rotator;
hinge_rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
hinge_axis = hinge_rotator*hinge_axis;
const xform& body_transform = structure->GetTransformation(this);
const vec3 anchor = body_transform.GetPosition() + GetOriginalOffset();
joint_id_ = physics->CreateHingeJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), anchor, hinge_axis);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], body_data_.bounce_);
physics->SetAngularMotorTurn(joint_id_, 0, 0);
//physics->GetAxis1(joint_id_, hinge_axis);
ok = true;
} else if (body_data_.joint_type_ == kJointSlider) {
// Calculate axis from given euler angles.
vec3 axis(0, 0, 1);
quat rotator;
rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
axis = rotator*axis;
joint_id_ = physics->CreateSliderJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), axis);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], body_data_.bounce_);
physics->SetMotorTarget(joint_id_, 0, 0);
ok = true;
} else if (body_data_.joint_type_ == kJointUniversal) {
// Calculate axis from given euler angles.
vec3 axis1(0, 0, 1);
vec3 axis2(0, 1, 0);
quat rotator;
rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
axis1 = rotator*axis1;
axis2 = rotator*axis2;
const xform& body_transform = structure->GetTransformation(this);
const vec3 anchor = body_transform.GetPosition() +
vec3(body_data_.parameter_[kParamOffsetX], body_data_.parameter_[kParamOffsetY], body_data_.parameter_[kParamOffsetZ]);
joint_id_ = physics->CreateUniversalJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), anchor, axis1, axis2);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
/*physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[0],
body_data_.parameter_[1]);
physics->SetAngularMotorRoll(joint_id_, 0, 0);
physics->SetAngularMotorTurn(joint_id_, 0, 0);*/
ok = true;
} else if (body_data_.joint_type_ == kJointBall) {
const xform& body_transform = structure->GetTransformation(this);
const vec3 anchor = body_transform.GetPosition() +
vec3(body_data_.parameter_[kParamOffsetX], body_data_.parameter_[kParamOffsetY], body_data_.parameter_[kParamOffsetZ]);
joint_id_ = physics->CreateBallJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), anchor);
/*physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[0],
body_data_.parameter_[1]);
physics->SetAngularMotorRoll(joint_id_, 0, 0);
physics->SetAngularMotorTurn(joint_id_, 0, 0);*/
ok = true;
} else {
deb_assert(false);
}
} else {
deb_assert(body_data_.joint_type_ == kJointExclude);
ok = true;
}
deb_assert(ok);
return (ok);
}
