|
World of Rigid Bodies (WoRB)
|
|
World of Rigid Bodies (WoRB)
|
Represents WoRB test-bed that can be initialized from mexFunction arguments.
More...
Public Member Functions | |
| WoRB_MexFunction () | |
| Default constructor. | |
| void | Parse (int nArgIn, const mxArray *argIn[]) |
| Initializes parameters and creates bodies from mexFunction arguments. | |
| mxArray * | GetResult () |
| Gets the result matrix. | |
| void | CreateResultMatrix () |
| Creates the result matrix. | |
| virtual void | OnProcessData () |
| Saves simulated data to be returned to MATLAB as the result. | |
Private Attributes | |
| Mex::Matrix | Result |
| Holds the results returned to MATLAB. | |
Represents WoRB test-bed that can be initialized from mexFunction arguments.
Definition at line 22 of file mexFunction.cpp.
| WoRB_MexFunction::WoRB_MexFunction | ( | ) | [inline] |
Default constructor.
Creates an empty result matrix.
Definition at line 32 of file mexFunction.cpp.
: Result( 0, 0 ) { }
| void WoRB_MexFunction::CreateResultMatrix | ( | ) | [inline] |
Creates the result matrix.
Definition at line 245 of file mexFunction.cpp.
References WoRB_TestBed::FinalTime, Mex::Matrix::GetM(), Mex::Matrix::GetN(), WoRB::Printf(), Result, and WoRB_TestBed::TimeStep.
Referenced by mexFunction().
{
// Ensure to have have a finite number of recorded time-steps
//
if ( FinalTime == 0 ) {
return;
}
unsigned n_steps = unsigned( FinalTime / TimeStep ) + 1;
Result = Mex::Matrix( n_steps, 11 );
// Set 'time' column to NaN (indicating row do not have valid data yet).
//
for ( unsigned i = 0; i < n_steps; ++i ) {
Result( i, 0 ) = mxGetNaN ();
}
WoRB::Printf( "WoRB: Created matrix for results [ %d × %d ]\n",
Result.GetM (), Result.GetN () );
}
| mxArray* WoRB_MexFunction::GetResult | ( | ) | [inline] |
Gets the result matrix.
Definition at line 238 of file mexFunction.cpp.
References Result.
Referenced by mexFunction().
{
return Result;
}
| virtual void WoRB_MexFunction::OnProcessData | ( | ) | [inline, virtual] |
Saves simulated data to be returned to MATLAB as the result.
Reimplemented from WoRB_TestBed.
Definition at line 268 of file mexFunction.cpp.
References WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::Collisions, WoRB::CollisionResolver::Count(), Mex::Matrix::IsEmpty(), WoRB_TestBed::Objects, Result, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::Time, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::TimeStepCount, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::TotalAngularMomentum, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::TotalKineticEnergy, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::TotalLinearMomentum, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::TotalPotentialEnergy, WoRB_TestBed::worb, WoRB::Quaternion::x, WoRB::Quaternion::y, and WoRB::Quaternion::z.
{
if ( Result.IsEmpty () ) {
return;
}
unsigned n = worb.TimeStepCount;
Result( n, 0 ) = worb.Time;
Result( n, 1 ) = worb.Collisions.Count ();
Result( n, 2 ) = worb.TotalKineticEnergy;
Result( n, 3 ) = worb.TotalPotentialEnergy;
Result( n, 4 ) = worb.TotalLinearMomentum.x;
Result( n, 5 ) = worb.TotalLinearMomentum.y;
Result( n, 6 ) = worb.TotalLinearMomentum.z;
Result( n, 7 ) = worb.TotalAngularMomentum.x;
Result( n, 8 ) = worb.TotalAngularMomentum.y;
Result( n, 9 ) = worb.TotalAngularMomentum.z;
Result( n, 10 ) = Objects.size () == 0 ? 0.0
: Objects.at( Objects.size() - 1 )->GetBody().Position.y;
}
| void WoRB_MexFunction::Parse | ( | int | nArgIn, |
| const mxArray * | argIn[] | ||
| ) | [inline] |
Initializes parameters and creates bodies from mexFunction arguments.
The first mex function argument should be a structure with fields as sys params. The second mex function argument should be a structure array with body parameters.
Definition at line 41 of file mexFunction.cpp.
References WoRB::GLUT_Renderer::ActiveColor, WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::Add(), WoRB_TestBed::AutoPause, WoRB_TestBed::CameraAngle, WoRB_TestBed::CameraElevation, WoRB_TestBed::CameraLookAt, WoRB_TestBed::CameraZoom, WoRB_TestBed::ClearTestBed(), WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::Collisions, WoRB::RigidBody::Deactivate(), WoRB_TestBed::Dump(), WoRB_TestBed::FinalTime, WoRB_TestBed::FollowObject, WoRB::CollisionResolver::Friction, WoRB::GLUT_Renderer::GetBody(), WoRB::GLUT_Renderer::GetGeometry(), WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::Gravity, WoRB_TestBed::GridTickLength, WoRB_TestBed::GridTicks, WoRB::GLUT_Renderer::InactiveColor, WoRB_TestBed::Initialize(), WoRB::WorldOfRigidBodies< MaxObjects, MaxCollisions >::InitializeODE(), WoRB_TestBed::IsInitialized, WoRB_TestBed::IsPaused, WoRB_TestBed::IsRunning, Mex::Matrix::IsSize(), WoRB_TestBed::Objects, WoRB::Printf(), WoRB::CollisionResolver::Relaxation, WoRB::CollisionResolver::Restitution, Result, WoRB::RigidBody::SetCanBeDeactivated(), WoRB::SevereError(), WoRB_TestBed::ShowBodyAxes, WoRB_TestBed::ShowContacts, WoRB_TestBed::ShowFloorMirror, WoRB_TestBed::ShowHelp, WoRB_TestBed::ShowStateVariables, WoRB_TestBed::ShowTrajectories, WoRB::GLUT_Renderer::ShowTrajectory, WoRB_TestBed::TestSuite, WoRB_TestBed::TimeStep, WoRB_TestBed::TimeStepsPerFrame, WoRB_TestBed::TimeStepsPerSnapshot, Mex::Matrix::VerifyDims(), WoRB_TestBed::WindowTitle, WoRB_TestBed::Wireframe, and WoRB_TestBed::worb.
Referenced by mexFunction().
{
if ( nArgIn >= 1 && ! mxIsStruct( argIn[0] ) ) {
WoRB::SevereError( "WoRB:Init:invarg",
"The first argument must be a structure with system parameters." );
}
if ( nArgIn >= 2 && ! mxIsStruct( argIn[1] ) ) {
WoRB::SevereError( "WoRB:Init:invarg",
"The second argument must be a structure array of body parameters." );
}
/////////////////////////////////////////////////////////////////////////////////
// Clear objects from previous simulations and make default parameters.
//
Result = Mex::Matrix( 0, 0 ); // Clear earlier results
Initialize (); // Initialize default parameters
ClearTestBed (); // Clear existing simulation, if any.
/////////////////////////////////////////////////////////////////////////////////
// Parse params structure
//
bool dumpInitialState = false;
if ( nArgIn >= 1 )
{
const mxArray* params = argIn[0];
WindowTitle = Mex::String( params, 0, "Title" );
TestSuite = Mex::Logical( params, 0, "TestSuite" );
IsInitialized = Mex::Logical( params, 0, "IsInitialized" );
IsRunning = Mex::Logical( params, 0, "IsRunning" );
IsPaused = Mex::Logical( params, 0, "IsPaused" );
AutoPause = Mex::Logical( params, 0, "AutoPause" );
Wireframe = Mex::Logical( params, 0, "Wireframe" );
ShowBodyAxes = Mex::Logical( params, 0, "ShowBodyAxes" );
ShowFloorMirror = Mex::Logical( params, 0, "ShowFloorMirror" );
ShowContacts = Mex::Logical( params, 0, "ShowContacts" );
ShowTrajectories = Mex::Logical( params, 0, "ShowTrajectories" );
ShowStateVariables = Mex::Logical( params, 0, "ShowStateVariables" );
ShowHelp = Mex::Logical( params, 0, "ShowHelp" );
dumpInitialState = Mex::Logical( params, 0, "DumpInitialState" );
GridTickLength = Mex::Scalar( params, 0, "GridTickLength" );
GridTicks = int( Mex::Scalar( params, 0, "GridTicks" ) );
TimeStep = Mex::Scalar( params, 0, "TimeStep" );
TimeStepsPerFrame = unsigned( Mex::Scalar( params, 0, "TimeStepsPerFrame" ) );
TimeStepsPerSnapshot = unsigned( Mex::Scalar( params, 0, "TimeStepsPerSnapshot" ) );
CameraAngle = Mex::Scalar( params, 0, "CameraAngle" );
CameraElevation = Mex::Scalar( params, 0, "CameraElevation" );
CameraZoom = Mex::Scalar( params, 0, "CameraZoom" );
Mex::Matrix lookAt( params, 0, "CameraLookAt" );
lookAt.VerifyDims( 1, 3, "CameraLookAt must be a spatial vector" );
CameraLookAt = WoRB::SpatialVector( lookAt(0), lookAt(1), lookAt(2) );
FollowObject = unsigned( Mex::Scalar( params, 0, "FollowObject" ) );
FinalTime = Mex::Scalar( params, 0, "FinalTime" );
worb.Collisions.Restitution = Mex::Scalar( params, 0, "Restitution" );
worb.Collisions.Relaxation = Mex::Scalar( params, 0, "Relaxation" );
worb.Collisions.Friction = Mex::Scalar( params, 0, "Friction" );
Mex::Matrix G( params, 0, "Gravity" );
G.VerifyDims( 1, 3, "Gravity must be a spatial vector" );
worb.Gravity = WoRB::SpatialVector( G(0), G(1), G(2) );
}
/////////////////////////////////////////////////////////////////////////////////
// Parse bodies array structure
//
if ( nArgIn >= 2 )
{
const mxArray* body = argIn[1];
unsigned bodyCount = unsigned( mxGetNumberOfElements( body ) );
for ( unsigned i = 0; i < bodyCount; ++i )
{
Mex::String geometry( body, i, "Geometry" ); // Geometry class
Mex::Matrix R( body, i, "HalfExtent" ); // Radius or half-extent
Mex::Matrix M( body, i, "M" ); // Body mass
Mex::Matrix X( body, i, "X" ); // Position
Mex::Matrix Q( body, i, "Q" ); // Oritentation
Mex::Matrix V( body, i, "V" ); // Velocity
Mex::Matrix W( body, i, "W" ); // Angular velocity
Mex::Logical showTrajectory ( body, i, "ShowTrajectory", true );
Mex::Logical isActive ( body, i, "CanBeDeactivated", true );
Mex::Logical canBeDeactivated ( body, i, "CanBeDeactivated", false );
Mex::Matrix cActive ( body, i, "ActiveColor" ); // Active color
Mex::Matrix cInactive( body, i, "InactiveColor" ); // Inactive color
// Verify dimensions of body parameters
//
M.VerifyDims( 1, 1, "Mass must be a scalar" );
X.VerifyDims( 1, 3, "Position must be a spatial vector" );
Q.VerifyDims( 1, 4, "Orientation must be a quaternion" );
V.VerifyDims( 1, 3, "Velocity must be a spatial vector" );
W.VerifyDims( 1, 3, "Angular velocity must be a spatial vector" );
// Create rigid bodies with geometries given by parameters
//
WoRB::GLUT_Renderer* obj = NULL;
if ( geometry == "cuboid" )
{
R.VerifyDims( 1, 3,
"Half-extent of a cuboid must be a spatial vector" );
obj = new WoRB::Box(
/* position */ WoRB::SpatialVector( X(0), X(1), X(2) ),
/* orientation */ WoRB::Quaternion ( Q(0), Q(1), Q(2), Q(3) ),
/* velocity */ WoRB::SpatialVector( V(0), V(1), V(2) ),
/* ang. velocity */ WoRB::SpatialVector( W(0), W(1), W(2) ),
/* half-extent */ WoRB::SpatialVector( R(0), R(1), R(2) ),
/* mass */ M(0)
);
}
else if ( geometry == "sphere" )
{
R.VerifyDims( 1, 1,
"Half-extent i.e. radius of a sphere must be a scalar" );
obj = new WoRB::Ball(
/* position */ WoRB::SpatialVector( X(0), X(1), X(2) ),
/* orientation */ WoRB::Quaternion ( Q(0), Q(1), Q(2), Q(3) ),
/* velocity */ WoRB::SpatialVector( V(0), V(1), V(2) ),
/* ang. velocity */ WoRB::SpatialVector( W(0), W(1), W(2) ),
/* radius */ R(0),
/* mass */ M(0)
);
}
else {
WoRB::SevereError( "WoRB:Init:invarg",
"Invalid body(%u) type '%s'; "
"allowed types are 'cuboid' or 'sphere'.",
i, (const char*)geometry );
}
// Optional trajectory flag
//
obj->ShowTrajectory = showTrajectory;
obj->GetBody().SetCanBeDeactivated( canBeDeactivated );
if ( ! isActive ) {
obj->GetBody().Deactivate ();
}
// Optional active color
//
if ( cActive.IsSize( 1, 4 ) ) {
obj->ActiveColor = WoRB::GLUT_Renderer::Colorf(
cActive(0), cActive(1), cActive(2), cActive(3)
);
}
// Optional inactive color
//
if ( cInactive.IsSize( 1, 4 ) ) {
obj->InactiveColor = WoRB::GLUT_Renderer::Colorf(
cInactive(0), cInactive(1), cInactive(2), cInactive(3)
);
}
// Add objects to the list
//
Objects.push_back( obj );
worb.Add( obj->GetGeometry () );
}
// Initialize ODE (calculate derived quantities for recently added objects)
//
worb.InitializeODE ();
// Clear test-suite configuration request since we've configured test-bed.
//
TestSuite = -1;
}
/////////////////////////////////////////////////////////////////////////////////
// Report parameters and bodies that we have so far.
//
if ( dumpInitialState )
{
Dump ();
WoRB::Printf( "\n" );
}
}
Mex::Matrix WoRB_MexFunction::Result [private] |
Holds the results returned to MATLAB.
Definition at line 26 of file mexFunction.cpp.
Referenced by CreateResultMatrix(), GetResult(), OnProcessData(), and Parse().
1.8.0