void CQTOpenGLUserFunctions::DrawPoint(const CVector3& c_position,
const CColor& c_color,
const Real f_diameter) {
/* Save attributes */
glPushAttrib(GL_POINT_BIT);
/* Set color */
SetColor(c_color);
/* Set point attributes */
glPointSize(f_diameter);
glEnable(GL_POINT_SMOOTH);
/* Draw */
glBegin(GL_POINTS);
glVertex3f(c_position.GetX(), c_position.GetY(), c_position.GetZ());
glEnd();
/* Restore saved attributes */
glPopAttrib();
}
void CRABMedium::Init(TConfigurationNode& t_tree) {
try {
CMedium::Init(t_tree);
/* Check occlusions? */
GetNodeAttributeOrDefault(t_tree, "check_occlusions", m_bCheckOcclusions, m_bCheckOcclusions);
/* Get the positional index method */
std::string strPosIndexMethod("grid");
GetNodeAttributeOrDefault(t_tree, "index", strPosIndexMethod, strPosIndexMethod);
/* Get the arena center and size */
CVector3 cArenaCenter;
CVector3 cArenaSize;
TConfigurationNode& tArena = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "arena");
GetNodeAttribute(tArena, "size", cArenaSize);
GetNodeAttributeOrDefault(tArena, "center", cArenaCenter, cArenaCenter);
/* Create the positional index for embodied entities */
if(strPosIndexMethod == "grid") {
size_t punGridSize[3];
if(!NodeAttributeExists(t_tree, "grid_size")) {
punGridSize[0] = cArenaSize.GetX();
punGridSize[1] = cArenaSize.GetY();
punGridSize[2] = cArenaSize.GetZ();
}
else {
std::string strPosGridSize;
GetNodeAttribute(t_tree, "grid_size", strPosGridSize);
ParseValues<size_t>(strPosGridSize, 3, punGridSize, ',');
}
CGrid<CRABEquippedEntity>* pcGrid = new CGrid<CRABEquippedEntity>(
cArenaCenter - cArenaSize * 0.5f, cArenaCenter + cArenaSize * 0.5f,
punGridSize[0], punGridSize[1], punGridSize[2]);
m_pcRABEquippedEntityGridUpdateOperation = new CRABEquippedEntityGridEntityUpdater(*pcGrid);
pcGrid->SetUpdateEntityOperation(m_pcRABEquippedEntityGridUpdateOperation);
m_pcRABEquippedEntityIndex = pcGrid;
}
else {
THROW_ARGOSEXCEPTION("Unknown method \"" << strPosIndexMethod << "\" for the positional index.");
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error in initialization of the range-and-bearing medium", ex);
}
}
void CQTOpenGLUserFunctions::DrawText(const CVector3& c_position,
const std::string& str_text,
const CColor& c_color) {
/* Save attributes */
glPushAttrib(GL_ENABLE_BIT);
/* Disable lighting to make text color unaffected by light */
glDisable(GL_LIGHTING);
/* Disable culling to make text visibile from any angle */
glDisable(GL_CULL_FACE);
/* Set color */
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
GetMainWindow().
GetOpenGLWidget().
renderText(c_position.GetX(),
c_position.GetY(),
c_position.GetZ(),
str_text.c_str());
/* Restore saved attributes */
glPopAttrib();
}
void CQTOpenGLCamera::SSettings::Translate(const CVector3& c_delta) {
Position += Forward * c_delta.GetX() + Left * c_delta.GetY() + Up * c_delta.GetZ();
Target = Position;
Target += Forward;
}
CDynamics2DBoxEntity::CDynamics2DBoxEntity(CDynamics2DEngine& c_engine,
CBoxEntity& c_entity) :
CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
m_cBoxEntity(c_entity),
m_fMass(c_entity.GetMass()),
m_ptShape(NULL),
m_ptBody(NULL) {
/* Get the size of the entity */
CVector3 cHalfSize = c_entity.GetSize() * 0.5f;
m_fHalfHeight = cHalfSize.GetZ();
/* Create a polygonal object in the physics space */
/* Start defining the vertices
NOTE: points must be defined in a clockwise winding
*/
cpVect tVertices[] = {
cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
cpv(-cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), -cHalfSize.GetY())
};
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
if(c_entity.GetEmbodiedEntity().IsMovable()) {
/* The box is movable */
/* Create the body */
m_ptBody =
cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
cpBodyNew(m_fMass,
cpMomentForPoly(m_fMass,
4,
tVertices,
cpvzero)));
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create the geometry */
m_ptShape =
cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_ptBody,
4,
tVertices,
cpvzero));
/* This object is grippable */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Lots contact friction to help pushing */
m_ptShape->u = 0.7;
/* Friction with ground */
m_ptLinearFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpPivotJointNew2(m_cEngine.GetGroundBody(),
m_ptBody,
cpvzero,
cpvzero));
m_ptLinearFriction->maxBias = 0.0f; // disable joint correction
m_ptLinearFriction->maxForce = 1.49f; // emulate linear friction (this is just slightly smaller than FOOTBOT_MAX_FORCE)
m_ptAngularFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpGearJointNew(m_cEngine.GetGroundBody(),
m_ptBody,
0.0f,
1.0f));
m_ptAngularFriction->maxBias = 0.0f; // disable joint correction
m_ptAngularFriction->maxForce = 1.49f; // emulate angular friction (this is just slightly smaller than FOOTBOT_MAX_TORQUE)
}
else {
/* The box is not movable */
/* Manually rotate the vertices */
cpVect tRot = cpvforangle(cZAngle.GetValue());
tVertices[0] = cpvrotate(tVertices[0], tRot);
tVertices[1] = cpvrotate(tVertices[1], tRot);
tVertices[2] = cpvrotate(tVertices[2], tRot);
tVertices[3] = cpvrotate(tVertices[3], tRot);
/* Create the geometry */
m_ptShape =
cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_cEngine.GetGroundBody(),
4,
tVertices,
cpv(cPosition.GetX(), cPosition.GetY())));
/* This object is normal */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Little contact friction to help sliding away */
m_ptShape->u = 0.1;
}
}
void CFootBotUN::ControlStep() {
printf("\n --------- UN %s - Simulation step %d -----------\n", GetRobot().GetRobotId().c_str(), CSimulator::GetInstance().GetSpace().GetSimulationClock());
m_pcLEDs->SetAllColors(CColor::BLUE);
/** Wifi communications section */
std::ostringstream str(ostringstream::out);
if (amIaGenerator) {
if (m_iSendOrNotSend % m_generatePacketInterval == 0) {
if (skipFirstSend) {
skipFirstSend = false;
} else {
sendPackets++;
str << "Hi I'm " << str_Me << " and I say \"Hello " << str_Dest << "\"";
str << ",Hi I'm " << str_Me << " and I say \"Hello " << str_Dest << "\"";
m_pcWifiActuator->SendMessageTo(str_Dest, str.str());
//std::cout << str_Me << " sending\n";
}
}
}
m_iSendOrNotSend++;
//searching for the received msgs
TMessageList t_incomingMsgs;
m_pcWifiSensor->GetReceivedMessages(t_incomingMsgs);
for (TMessageList::iterator it = t_incomingMsgs.begin(); it != t_incomingMsgs.end(); it++) {
receivedPackets++;
std::cout << str_Me << " received: " << it->Payload << " from " << it->Sender << " (total received=)" << receivedPackets << std::endl;
}
/** End of wifi*/
/** LCM for getting current positions and obtaining the next target point */
UInt8 robotID = atoi(GetRobot().GetRobotId().substr(3).c_str());
/* New target point from the COMMAND engine*/
if (lcmThreadCommand.getLcmHandler()->existNode(robotID)) {
Node nodeCommand = lcmThreadCommand.getLcmHandler()->getNodeById(robotID);
targetPosition.Set(nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
printf("ID %d - TARGET: (%f,%f)\n", robotID, nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
} else {
if (lcmThread.getLcmHandler()->existNode(robotID)) {
Node nodeCommand = lcmThread.getLcmHandler()->getNodeById(robotID);
targetPosition.Set(nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
printf("ID %d - TARGET: (%f,%f)\n", robotID, nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
}
}
if (lcmThread.getLcmHandler()->existNode(robotID)) {
//lcmThread.getLcmHandler()->printNodeListElements();
/** LCM related Node information */
//To get the other nodes locations through LCM
listNodeObstacles = lcmThread.getLcmHandler()->retrieveNodeList();
//Get myself
mySelf = lcmThread.getLcmHandler()->getNodeById(robotID);
printf("ID %d\n", mySelf.getId());
printf("POS (%f,%f)\n", mySelf.getPosition().GetX(), mySelf.getPosition().GetY());
printf("QUAT (%f,%f,%f,%f)\n", mySelf.getOrientation().GetW(), mySelf.getOrientation().GetX(), mySelf.getOrientation().GetY(), mySelf.getOrientation().GetZ());
printf("VEL %d\n", mySelf.getVelocity());
/** From the simulator */
CVector2 oldPosition = position;
CSpace& space = CSimulator::GetInstance().GetSpace();
const std::string stringIdA = GetRobot().GetRobotId();
CFootBotEntity *robotEntityA = static_cast<CFootBotEntity*>(&space.GetEntity(stringIdA));
CVector3 cFootBotPositionA = robotEntityA->GetEmbodiedEntity().GetPosition();
CQuaternion cFootBotOrientationA = robotEntityA->GetEmbodiedEntity().GetOrientation();
CVector3 axis;
CRadians oA;
cFootBotOrientationA.ToAngleAxis(oA, axis);
if (axis.GetZ() < 0)
oA = -oA;
// Position
position = CVector2(cFootBotPositionA.GetX(), cFootBotPositionA.GetY());
// Angle
angle = oA;
// Velocity
//velocity = CVector2(speed, 0);
velocity = CVector2(mySelf.getVelocity(), 0);
/** NAVIGATION AND AVOIDING COLLISION */
updateAgent(agent);
agent->clearObstacles();
addNodesAsObstacles(listNodeObstacles);
if ((targetPosition - position).Length() < m_targetMinPointDistance) { // 2 cm
state = ARRIVED_AT_TARGET;
printf("State: STOPPED\n");
} else {
state = MOVING;
printf("State: MOVING\n");
}
updateNavigation();
}
}
void CQTOpenGLUserFunctions::DrawCylinder(Real f_radius,
Real f_height,
const CVector3& c_center_offset,
const CColor& c_color,
const CQuaternion& c_orientation,
GLuint un_vertices) {
/* Draw top circle*/
CVector3 cCirclePos(0.0f, 0.0f, f_height * 0.5f);
cCirclePos.Rotate(c_orientation);
cCirclePos += c_center_offset;
DrawCircle(f_radius,
cCirclePos,
c_color,
true,
c_orientation,
un_vertices);
/* Draw bottom circle*/
cCirclePos.Set(0.0f, 0.0f, -f_height * 0.5f);
cCirclePos.Rotate(c_orientation);
cCirclePos += c_center_offset;
DrawCircle(f_radius,
cCirclePos,
c_color,
true,
c_orientation,
un_vertices);
/* Side surface */
CVector3 cVertex1(f_radius, 0.0f, f_height * 0.5f);
CVector3 cVertex2(f_radius, 0.0f, -f_height * 0.5f);
CRadians cAngle(CRadians::TWO_PI / un_vertices);
/* Compute the quaternion defining the rotation of the vertices used to draw the side surface. */
CQuaternion cVertexRotation;
CVector3 cVertexRotationAxis(0.0f, 0.0f, 1.0f);
cVertexRotationAxis.Rotate(c_orientation);
cVertexRotation.FromAngleAxis(cAngle, cVertexRotationAxis);
glDisable(GL_LIGHTING);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
glBegin(GL_QUAD_STRIP);
/* Compute the normal direction of the starting edge. */
CVector3 cNormalDirection(cVertex1.GetX(), cVertex1.GetY(), 0.0f);
cNormalDirection.Rotate(c_orientation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
/* Rotate the endpoints of the first edge.*/
cVertex1.Rotate(c_orientation);
cVertex2.Rotate(c_orientation);
for(GLuint i = 0; i <= un_vertices; i++) {
glVertex3f(cVertex1.GetX() + c_center_offset.GetX(),
c_center_offset.GetY() + cVertex1.GetY(),
c_center_offset.GetZ() + cVertex1.GetZ() );
glVertex3f(cVertex2.GetX() + c_center_offset.GetX(),
c_center_offset.GetY() + cVertex2.GetY(),
c_center_offset.GetZ() + cVertex2.GetZ() );
/* Rotate the vertices and the normal direction, set the new normal. */
cVertex1.Rotate(cVertexRotation);
cVertex2.Rotate(cVertexRotation);
cNormalDirection.Rotate(cVertexRotation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
}
glEnd();
glEnable(GL_LIGHTING);
}
void CQTOpenGLUserFunctions::DrawBox(const CVector3& c_position,
const CQuaternion& c_orientation,
const CVector3& c_size,
const CColor& c_color) {
/* Save current matrix */
glPushMatrix();
/* Set color */
SetColor(c_color);
/* Set position/orientation */
Rototranslate(c_position, c_orientation);
/* Draw top and bottom faces (parallel to XY) */
CVector3 cHalfSize = c_size * 0.5f;
glBegin(GL_QUADS);
/* Bottom face */
glNormal3f(0.0f, 0.0f, -1.0f);
glVertex3f( cHalfSize.GetX(), cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), -cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), -cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), cHalfSize.GetY(), -cHalfSize.GetZ());
/* Top face */
glNormal3f(0.0f, 0.0f, 1.0f);
glVertex3f(-cHalfSize.GetX(), -cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), -cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), cHalfSize.GetY(), cHalfSize.GetZ());
glEnd();
/* Draw the other faces (South, East, North, West) */
glBegin(GL_QUADS);
/* North face */
glNormal3f(1.0f, 0.0f, 0.0f);
glVertex3f( cHalfSize.GetX(), -cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), -cHalfSize.GetY(), cHalfSize.GetZ());
/* South face */
glNormal3f(-1.0f, 0.0f, 0.0f);
glVertex3f(-cHalfSize.GetX(), -cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), -cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), cHalfSize.GetY(), -cHalfSize.GetZ());
/* East face */
glNormal3f(0.0f, -1.0f, 0.0f);
glVertex3f(-cHalfSize.GetX(), -cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), -cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), -cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), -cHalfSize.GetY(), cHalfSize.GetZ());
/* West face */
glNormal3f(0.0f, 1.0f, 0.0f);
glVertex3f(-cHalfSize.GetX(), cHalfSize.GetY(), -cHalfSize.GetZ());
glVertex3f(-cHalfSize.GetX(), cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), cHalfSize.GetY(), cHalfSize.GetZ());
glVertex3f( cHalfSize.GetX(), cHalfSize.GetY(), -cHalfSize.GetZ());
glEnd();
/* Restore saved matrix */
glPopMatrix();
}
///////////////////////////////////////////////////////////////
// METHODS
///////////////////////////////////////////////////////////////
// GetIKTransform =============================================
CTransformation GetIKTransform(s_GetIKTransform values, CString outportName)
{
// prepare all variables
CTransformation result;
CVector3 bonePos,rootPos,effPos,upvPos,rootEff, xAxis,yAxis,zAxis, rollAxis;
rootPos = values.root.GetTranslation();
effPos = values.eff.GetTranslation();
upvPos = values.upv.GetTranslation();
rootEff.Sub(effPos,rootPos);
rollAxis.Normalize(rootEff);
CMatrix4 rootMatrix = values.root.GetMatrix4();
CMatrix4 rootMatrixNeg;
rootMatrixNeg.Invert(rootMatrix);
double rootEffDistance = rootEff.GetLength();
// init the scaling
double global_scale = values.root.GetScaling().GetX();
result.SetScaling(values.root.GetScaling());
// Distance with MaxStretch ---------------------
double restLength = values.lengthA * values.scaleA + values.lengthB * values.scaleB;
CVector3 distanceVector;
distanceVector.MulByMatrix4(effPos, rootMatrixNeg);
double distance = distanceVector.GetLength();
double distance2 = distance;
if (distance > (restLength * (values.maxstretch)))
{
distanceVector.NormalizeInPlace();
distanceVector.ScaleInPlace(restLength * (values.maxstretch) );
distance = restLength * (values.maxstretch);
}
// Adapt Softness value to chain length --------
values.softness = values.softness * restLength *.1;
// Stretch and softness ------------------------
// We use the real distance from root to controler to calculate the softness
// This way we have softness working even when there is no stretch
double stretch = max(1, distance / restLength);
double da = restLength - values.softness;
if (values.softness > 0 && distance2 > da)
{
double newlen = values.softness*(1.0 - exp(-(distance2 -da)/values.softness)) + da;
stretch = distance / newlen;
}
values.lengthA = values.lengthA * stretch * values.scaleA * global_scale;
values.lengthB = values.lengthB * stretch * values.scaleB * global_scale;
// Reverse -------------------------------------
double d = distance / (values.lengthA + values.lengthB);
double reverse_scale;
if (values.reverse < 0.5)
reverse_scale = 1-(values.reverse*2 * (1-d));
else
reverse_scale = 1-((1-values.reverse)*2 * (1-d));
values.lengthA *= reverse_scale;
values.lengthB *= reverse_scale;
bool invert = values.reverse > 0.5;
// Slide ---------------------------------------
double slide_add;
if (values.slide < .5)
slide_add = (values.lengthA * (values.slide * 2)) - (values.lengthA);
else
slide_add = (values.lengthB * (values.slide * 2)) - (values.lengthB);
values.lengthA += slide_add;
values.lengthB -= slide_add;
// calculate the angle inside the triangle!
double angleA = 0;
double angleB = 0;
// check if the divider is not null otherwise the result is nan
// and the output disapear from xsi, that breaks constraints
if((rootEffDistance < values.lengthA + values.lengthB) && (rootEffDistance > fabs(values.lengthA - values.lengthB) + 1E-6))
{
// use the law of cosine for lengthA
double a = values.lengthA;
double b = rootEffDistance;
double c = values.lengthB;
angleA = acos(min(1, (a * a + b * b - c * c ) / ( 2 * a * b)));
// use the law of cosine for lengthB
a = values.lengthB;
b = values.lengthA;
c = rootEffDistance;
angleB = acos(min(1, (a * a + b * b - c * c ) / ( 2 * a * b)));
// invert the angles if need be
if(invert)
{
angleA = -angleA;
angleB = -angleB;
}
}
// start with the X and Z axis
xAxis = rootEff;
yAxis.LinearlyInterpolate(rootPos,effPos,.5);
yAxis.Sub(upvPos,yAxis);
yAxis = rotateVectorAlongAxis(yAxis, rollAxis, values.roll);
zAxis.Cross(xAxis,yAxis);
xAxis.NormalizeInPlace();
zAxis.NormalizeInPlace();
// switch depending on our mode
if(outportName == "OutBoneA") // Bone A
{
// check if we need to rotate the bone
if(angleA != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleA);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
}
if (values.negate)
xAxis.NegateInPlace();
// cross the yAxis and normalize
yAxis.Cross(zAxis,xAxis);
yAxis.NormalizeInPlace();
// output the rotation
result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);
// set the scaling + the position
result.SetSclX(values.lengthA);
result.SetTranslation(rootPos);
}
else if(outportName == "OutBoneB") // Bone B
{
// check if we need to rotate the bone
if(angleA != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleA);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
}
// calculate the position of the elbow!
bonePos.Scale(values.lengthA,xAxis);
bonePos.AddInPlace(rootPos);
// check if we need to rotate the bone
if(angleB != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleB-XSI::MATH::PI);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
}
if (values.negate)
xAxis.NegateInPlace();
// cross the yAxis and normalize
yAxis.Cross(zAxis,xAxis);
yAxis.NormalizeInPlace();
// output the rotation
result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);
// set the scaling + the position
result.SetSclX(values.lengthB);
result.SetTranslation(bonePos);
}
else if(outportName == "OutCenter") // center
{
// check if we need to rotate the bone
bonePos.Scale(values.lengthA,xAxis);
if(angleA != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleA);
bonePos.MulByMatrix3InPlace(rot.GetMatrix());
}
bonePos.AddInPlace(rootPos);
// cross the yAxis and normalize
yAxis.Sub(upvPos,bonePos);
zAxis.Cross(xAxis,yAxis);
zAxis.NormalizeInPlace();
if (values.negate)
xAxis.NegateInPlace();
yAxis.Cross(zAxis,xAxis);
yAxis.NormalizeInPlace();
// output the rotation
result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);
// set the scaling + the position
result.SetSclX(stretch * values.root.GetSclX());
result.SetTranslation(bonePos);
}
else if(outportName == "OutCenterN") // center normalized
{
// check if we need to rotate the bone
if(angleA != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleA);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
}
// calculate the position of the elbow!
bonePos.Scale(values.lengthA,xAxis);
bonePos.AddInPlace(rootPos);
// check if we need to rotate the bone
if(angleB != 0.0)
{
if(invert)
angleB += XSI::MATH::PI * 2;
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleB*.5-XSI::MATH::PI*.5);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
} // cross the yAxis and normalize
// yAxis.Sub(upvPos,bonePos); // this was flipping the centerN when the elbow/upv was aligned to root/eff
zAxis.Cross(xAxis,yAxis);
zAxis.NormalizeInPlace();
if (values.negate)
xAxis.NegateInPlace();
yAxis.Cross(zAxis,xAxis);
yAxis.NormalizeInPlace();
// output the rotation
result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);
// set the scaling + the position
// result.SetSclX(stretch * values.root.GetSclX());
result.SetTranslation(bonePos);
}
else if(outportName == "OutEff") // effector
{
// check if we need to rotate the bone
effPos = rootPos;
if(angleA != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleA);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
}
// calculate the position of the elbow!
bonePos.Scale(values.lengthA,xAxis);
effPos.AddInPlace(bonePos);
// check if we need to rotate the bone
if(angleB != 0.0)
{
CRotation rot;
rot.SetFromAxisAngle(zAxis,angleB-XSI::MATH::PI);
xAxis.MulByMatrix3InPlace(rot.GetMatrix());
}
// calculate the position of the effector!
bonePos.Scale(values.lengthB,xAxis);
effPos.AddInPlace(bonePos);
// cross the yAxis and normalize
yAxis.Cross(zAxis,xAxis);
yAxis.NormalizeInPlace();
// output the rotation
result = values.eff;
result.SetTranslation(effPos);
}
CRotation r = result.GetRotation();
CVector3 eulerAngles = r.GetXYZAngles();
double rx = eulerAngles.GetX();
double ry = eulerAngles.GetY();
double rz = eulerAngles.GetZ();
return result;
}
SICALLBACK MOM_GetAttributes_Evaluate( ICENodeContext& in_ctxt )
{
// The current output port being evaluated...
ULONG out_portID = in_ctxt.GetEvaluatedOutputPortID( );
CDataArrayLong baseData( in_ctxt, ID_IN_base );
CDataArrayLong idData( in_ctxt, ID_IN_id );
rbdID rbd_ID;
CIndexSet indexSet( in_ctxt );
if(gSimulation == NULL)
return CStatus::OK;
switch( out_portID )
{
case ID_OUT_position :
{
// Get the output port array ...
CDataArrayVector3f outData( in_ctxt );
// get the index set iterator
btTransform bodyTransform;
btVector3 bodyPos;
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
{
bodyRef->body->getMotionState()->getWorldTransform(bodyTransform);
bodyPos = bodyTransform.getOrigin();
outData[it] = CVector3f(bodyPos.getX(),bodyPos.getY(),bodyPos.getZ());
}
}
break;
}
case ID_OUT_orientation :
{
// Get the output port array ...
CDataArrayVector3f outData( in_ctxt );
// get the index set iterator
btTransform bodyTransform;
btQuaternion bodyRot;
CRotation rot;
CVector3 angles;
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
{
bodyTransform = bodyRef->GetWorldTransform();
bodyRot = bodyTransform.getRotation();
rot.SetFromQuaternion(CQuaternion(bodyRot.getW(),bodyRot.getX(),bodyRot.getY(),bodyRot.getZ()));
angles = rot.GetXYZAngles();
outData[it].Set(RadiansToDegrees(angles.GetX()),RadiansToDegrees(angles.GetY()),RadiansToDegrees(angles.GetZ()));
}
}
break;
}
case ID_OUT_linvelocity:
{
// Get the output port array ...
CDataArrayVector3f outData( in_ctxt );
// get the index set iterator
btVector3 linvel;
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
{
linvel = bodyRef->body->getLinearVelocity();
outData[it].Set(linvel.getX(),linvel.getY(),linvel.getZ());
}
}
break;
}
case ID_OUT_angvelocity:
{
// Get the output port array ...
CDataArrayVector3f outData( in_ctxt );
// get the index set iterator
btVector3 angvel;
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
{
angvel = bodyRef->body->getAngularVelocity();
outData[it].Set(angvel.getX(),angvel.getY(),angvel.getZ());
}
}
break;
}
case ID_OUT_state:
{
// Get the output port array ...
CDataArrayLong outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
{
outData[it] = 0;
if(bodyRef->body->getActivationState() == WANTS_DEACTIVATION)
outData[it] = 1;
else if(bodyRef->body->getActivationState() == DISABLE_SIMULATION)
outData[it] = 2;
}
}
break;
}
case ID_OUT_mass:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = 1.0f / bodyRef->body->getInvMass();
}
break;
}
case ID_OUT_bounce:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = bodyRef->body->getRestitution();
}
break;
}
case ID_OUT_friction:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = bodyRef->body->getFriction();
}
break;
}
case ID_OUT_lindamping:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = bodyRef->body->getLinearDamping();
}
break;
}
case ID_OUT_angdamping:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = bodyRef->body->getAngularDamping();
}
break;
}
case ID_OUT_lintreshold:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = bodyRef->body->getLinearSleepingThreshold();
}
break;
}
case ID_OUT_angtreshold:
{
// Get the output port array ...
CDataArrayFloat outData( in_ctxt );
// get the index set iterator
for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next())
{
rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]);
rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]);
btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID);
if(bodyRef != NULL)
outData[it] = bodyRef->body->getAngularSleepingThreshold();
}
break;
}
};
return CStatus::OK;
}
XSI::CStatus AlembicSubD::Save(double time)
{
// store the transform
Primitive prim(GetRef(REF_PRIMITIVE));
bool globalSpace = GetJob()->GetOption(L"globalSpace");
// query the global space
CTransformation globalXfo;
if(globalSpace)
globalXfo = KinematicState(GetRef(REF_GLOBAL_TRANS)).GetTransform(time);
CTransformation globalRotation;
globalRotation.SetRotation(globalXfo.GetRotation());
// store the metadata
SaveMetaData(GetRef(REF_NODE),this);
// check if the mesh is animated
if(mNumSamples > 0) {
if(!isRefAnimated(GetRef(REF_PRIMITIVE), false, globalSpace))
return CStatus::OK;
}
// determine if we are a pure point cache
bool purePointCache = (bool)GetJob()->GetOption(L"exportPurePointCache");
// access the mesh
PolygonMesh mesh = prim.GetGeometry(time);
CVector3Array pos = mesh.GetVertices().GetPositionArray();
LONG vertCount = pos.GetCount();
// prepare the bounding box
Abc::Box3d bbox;
// allocate the points and normals
std::vector<Abc::V3f> posVec(vertCount);
for(LONG i=0;i<vertCount;i++)
{
if(globalSpace)
pos[i] = MapObjectPositionToWorldSpace(globalXfo,pos[i]);
posVec[i].x = (float)pos[i].GetX();
posVec[i].y = (float)pos[i].GetY();
posVec[i].z = (float)pos[i].GetZ();
bbox.extendBy(posVec[i]);
}
// allocate the sample for the points
if(posVec.size() == 0)
{
bbox.extendBy(Abc::V3f(0,0,0));
posVec.push_back(Abc::V3f(FLT_MAX,FLT_MAX,FLT_MAX));
}
Abc::P3fArraySample posSample(&posVec.front(),posVec.size());
// store the positions && bbox
mSubDSample.setPositions(posSample);
mSubDSample.setSelfBounds(bbox);
customAttributes.exportCustomAttributes(mesh);
// abort here if we are just storing points
if(purePointCache)
{
if(mNumSamples == 0)
{
// store a dummy empty topology
mFaceCountVec.push_back(0);
mFaceIndicesVec.push_back(0);
Abc::Int32ArraySample faceCountSample(&mFaceCountVec.front(),mFaceCountVec.size());
Abc::Int32ArraySample faceIndicesSample(&mFaceIndicesVec.front(),mFaceIndicesVec.size());
mSubDSample.setFaceCounts(faceCountSample);
mSubDSample.setFaceIndices(faceIndicesSample);
}
mSubDSchema.set(mSubDSample);
mNumSamples++;
return CStatus::OK;
}
// check if we support changing topology
bool dynamicTopology = (bool)GetJob()->GetOption(L"exportDynamicTopology");
CPolygonFaceRefArray faces = mesh.GetPolygons();
LONG faceCount = faces.GetCount();
LONG sampleCount = mesh.GetSamples().GetCount();
// create a sample look table
LONG offset = 0;
CLongArray sampleLookup(sampleCount);
for(LONG i=0;i<faces.GetCount();i++)
{
PolygonFace face(faces[i]);
CLongArray samples = face.GetSamples().GetIndexArray();
for(LONG j=samples.GetCount()-1;j>=0;j--)
sampleLookup[offset++] = samples[j];
}
// check if we should export the velocities
if(dynamicTopology)
{
ICEAttribute velocitiesAttr = mesh.GetICEAttributeFromName(L"PointVelocity");
if(velocitiesAttr.IsDefined() && velocitiesAttr.IsValid())
{
CICEAttributeDataArrayVector3f velocitiesData;
velocitiesAttr.GetDataArray(velocitiesData);
mVelocitiesVec.resize(vertCount);
for(LONG i=0;i<vertCount;i++)
{
CVector3 vel;
vel.PutX(velocitiesData[i].GetX());
vel.PutY(velocitiesData[i].GetY());
vel.PutZ(velocitiesData[i].GetZ());
if(globalSpace)
vel = MapObjectPositionToWorldSpace(globalRotation,vel);
mVelocitiesVec[i].x = (float)vel.GetX();
mVelocitiesVec[i].y = (float)vel.GetY();
mVelocitiesVec[i].z = (float)vel.GetZ();
}
if(mVelocitiesVec.size() == 0)
mVelocitiesVec.push_back(Abc::V3f(0,0,0));
Abc::V3fArraySample sample = Abc::V3fArraySample(&mVelocitiesVec.front(),mVelocitiesVec.size());
mSubDSample.setVelocities(sample);
}
}
// if we are the first frame!
if(mNumSamples == 0 || dynamicTopology)
{
// we also need to store the face counts as well as face indices
if(mFaceIndicesVec.size() != sampleCount || sampleCount == 0)
{
mFaceCountVec.resize(faceCount);
mFaceIndicesVec.resize(sampleCount);
offset = 0;
for(LONG i=0;i<faceCount;i++)
{
PolygonFace face(faces[i]);
CLongArray indices = face.GetVertices().GetIndexArray();
mFaceCountVec[i] = indices.GetCount();
for(LONG j=indices.GetCount()-1;j>=0;j--)
mFaceIndicesVec[offset++] = indices[j];
}
if(mFaceIndicesVec.size() == 0)
{
mFaceCountVec.push_back(0);
mFaceIndicesVec.push_back(0);
}
Abc::Int32ArraySample faceCountSample(&mFaceCountVec.front(),mFaceCountVec.size());
Abc::Int32ArraySample faceIndicesSample(&mFaceIndicesVec.front(),mFaceIndicesVec.size());
mSubDSample.setFaceCounts(faceCountSample);
mSubDSample.setFaceIndices(faceIndicesSample);
}
// set the subd level
Property geomApproxProp;
prim.GetParent3DObject().GetPropertyFromName(L"geomapprox",geomApproxProp);
mSubDSample.setFaceVaryingInterpolateBoundary(geomApproxProp.GetParameterValue(L"gapproxmordrsl"));
// also check if we need to store UV
CRefArray clusters = mesh.GetClusters();
if((bool)GetJob()->GetOption(L"exportUVs"))
{
CGeometryAccessor accessor = mesh.GetGeometryAccessor(siConstructionModeSecondaryShape);
CRefArray uvPropRefs = accessor.GetUVs();
// if we now finally found a valid uvprop
if(uvPropRefs.GetCount() > 0)
{
// ok, great, we found UVs, let's set them up
if(mNumSamples == 0)
{
mUvVec.resize(uvPropRefs.GetCount());
if((bool)GetJob()->GetOption(L"indexedUVs"))
mUvIndexVec.resize(uvPropRefs.GetCount());
// query the names of all uv properties
std::vector<std::string> uvSetNames;
for(LONG i=0;i< uvPropRefs.GetCount();i++)
uvSetNames.push_back(ClusterProperty(uvPropRefs[i]).GetName().GetAsciiString());
Abc::OStringArrayProperty uvSetNamesProperty = Abc::OStringArrayProperty(
mSubDSchema, ".uvSetNames", mSubDSchema.getMetaData(), GetJob()->GetAnimatedTs() );
Abc::StringArraySample uvSetNamesSample(&uvSetNames.front(),uvSetNames.size());
uvSetNamesProperty.set(uvSetNamesSample);
}
// loop over all uvsets
for(LONG uvI=0;uvI<uvPropRefs.GetCount();uvI++)
{
mUvVec[uvI].resize(sampleCount);
CDoubleArray uvValues = ClusterProperty(uvPropRefs[uvI]).GetElements().GetArray();
for(LONG i=0;i<sampleCount;i++)
{
mUvVec[uvI][i].x = (float)uvValues[sampleLookup[i] * 3 + 0];
mUvVec[uvI][i].y = (float)uvValues[sampleLookup[i] * 3 + 1];
}
// now let's sort the normals
size_t uvCount = mUvVec[uvI].size();
size_t uvIndexCount = 0;
if((bool)GetJob()->GetOption(L"indexedUVs")) {
std::map<SortableV2f,size_t> uvMap;
std::map<SortableV2f,size_t>::const_iterator it;
size_t sortedUVCount = 0;
std::vector<Abc::V2f> sortedUVVec;
mUvIndexVec[uvI].resize(mUvVec[uvI].size());
sortedUVVec.resize(mUvVec[uvI].size());
// loop over all uvs
for(size_t i=0;i<mUvVec[uvI].size();i++)
{
it = uvMap.find(mUvVec[uvI][i]);
if(it != uvMap.end())
mUvIndexVec[uvI][uvIndexCount++] = (Abc::uint32_t)it->second;
else
{
mUvIndexVec[uvI][uvIndexCount++] = (Abc::uint32_t)sortedUVCount;
uvMap.insert(std::pair<Abc::V2f,size_t>(mUvVec[uvI][i],(Abc::uint32_t)sortedUVCount));
sortedUVVec[sortedUVCount++] = mUvVec[uvI][i];
}
}
// use indexed uvs if they use less space
mUvVec[uvI] = sortedUVVec;
uvCount = sortedUVCount;
sortedUVCount = 0;
sortedUVVec.clear();
}
AbcG::OV2fGeomParam::Sample uvSample(Abc::V2fArraySample(&mUvVec[uvI].front(),uvCount),AbcG::kFacevaryingScope);
if(mUvIndexVec.size() > 0 && uvIndexCount > 0)
uvSample.setIndices(Abc::UInt32ArraySample(&mUvIndexVec[uvI].front(),uvIndexCount));
if(uvI == 0)
{
mSubDSample.setUVs(uvSample);
}
else
{
// create the uv param if required
if(mNumSamples == 0)
{
CString storedUvSetName = CString(L"uv") + CString(uvI);
mUvParams.push_back(AbcG::OV2fGeomParam( mSubDSchema, storedUvSetName.GetAsciiString(), uvIndexCount > 0,
AbcG::kFacevaryingScope, 1, GetJob()->GetAnimatedTs()));
}
mUvParams[uvI-1].set(uvSample);
}
}
// create the uv options
if(mUvOptionsVec.size() == 0)
{
mUvOptionsProperty = Abc::OFloatArrayProperty(mSubDSchema, ".uvOptions", mSubDSchema.getMetaData(), GetJob()->GetAnimatedTs() );
for(LONG uvI=0;uvI<uvPropRefs.GetCount();uvI++)
{
//ESS_LOG_ERROR( "Cluster Property child name: " << ClusterProperty(uvPropRefs[uvI]).GetFullName().GetAsciiString() );
//ESS_LOG_ERROR( "Cluster Property child type: " << ClusterProperty(uvPropRefs[uvI]).GetType().GetAsciiString() );
ClusterProperty clusterProperty = (ClusterProperty) uvPropRefs[uvI];
bool subdsmooth = false;
if( clusterProperty.GetType() == L"uvspace") {
subdsmooth = (bool)clusterProperty.GetParameter(L"subdsmooth").GetValue();
// ESS_LOG_ERROR( "subdsmooth: " << subdsmooth );
}
CRefArray children = clusterProperty.GetNestedObjects();
bool uWrap = false;
bool vWrap = false;
for(LONG i=0; i<children.GetCount(); i++)
{
ProjectItem child(children.GetItem(i));
CString type = child.GetType();
// ESS_LOG_ERROR( " Cluster Property child type: " << type.GetAsciiString() );
if(type == L"uvprojdef")
{
uWrap = (bool)child.GetParameter(L"wrap_u").GetValue();
vWrap = (bool)child.GetParameter(L"wrap_v").GetValue();
break;
}
}
// uv wrapping
mUvOptionsVec.push_back(uWrap ? 1.0f : 0.0f);
mUvOptionsVec.push_back(vWrap ? 1.0f : 0.0f);
mUvOptionsVec.push_back(subdsmooth ? 1.0f : 0.0f);
}
mUvOptionsProperty.set(Abc::FloatArraySample(&mUvOptionsVec.front(),mUvOptionsVec.size()));
}
}
}
// sweet, now let's have a look at face sets
if(GetJob()->GetOption(L"exportFaceSets") && mNumSamples == 0)
{
for(LONG i=0;i<clusters.GetCount();i++)
{
Cluster cluster(clusters[i]);
if(!cluster.GetType().IsEqualNoCase(L"poly"))
continue;
CLongArray elements = cluster.GetElements().GetArray();
if(elements.GetCount() == 0)
continue;
std::string name(cluster.GetName().GetAsciiString());
mFaceSetsVec.push_back(std::vector<Abc::int32_t>());
std::vector<Abc::int32_t> & faceSetVec = mFaceSetsVec.back();
for(LONG j=0;j<elements.GetCount();j++)
faceSetVec.push_back(elements[j]);
if(faceSetVec.size() > 0)
{
AbcG::OFaceSet faceSet = mSubDSchema.createFaceSet(name);
AbcG::OFaceSetSchema::Sample faceSetSample(Abc::Int32ArraySample(&faceSetVec.front(),faceSetVec.size()));
faceSet.getSchema().set(faceSetSample);
}
}
}
// save the sample
mSubDSchema.set(mSubDSample);
// check if we need to export the bindpose
if(GetJob()->GetOption(L"exportBindPose") && prim.GetParent3DObject().GetEnvelopes().GetCount() > 0 && mNumSamples == 0)
{
mBindPoseProperty = Abc::OV3fArrayProperty(mSubDSchema, ".bindpose", mSubDSchema.getMetaData(), GetJob()->GetAnimatedTs());
// store the positions of the modeling stack into here
PolygonMesh bindPoseGeo = prim.GetGeometry(time, siConstructionModeModeling);
CVector3Array bindPosePos = bindPoseGeo.GetPoints().GetPositionArray();
mBindPoseVec.resize((size_t)bindPosePos.GetCount());
for(LONG i=0;i<bindPosePos.GetCount();i++)
{
mBindPoseVec[i].x = (float)bindPosePos[i].GetX();
mBindPoseVec[i].y = (float)bindPosePos[i].GetY();
mBindPoseVec[i].z = (float)bindPosePos[i].GetZ();
}
Abc::V3fArraySample sample;
if(mBindPoseVec.size() > 0)
sample = Abc::V3fArraySample(&mBindPoseVec.front(),mBindPoseVec.size());
mBindPoseProperty.set(sample);
}
}
else
{
mSubDSchema.set(mSubDSample);
}
mNumSamples++;
return CStatus::OK;
}
bool CEntityPhysics::Intersect(const CVector3& a,const CVector3& b,const CVector3& c,const CVector3& d)
{
float den = ((d.GetZ()-c.GetZ())*(b.GetX()-a.GetX())-(d.GetX()-c.GetX())*(b.GetZ()-a.GetZ()));
float num1 = ((d.GetX() - c.GetX())*(a.GetZ()-c.GetZ()) - (d.GetZ()- c.GetZ())*(a.GetX()-c.GetX()));
float num2 = ((b.GetX()-a.GetX())*(a.GetZ()-c.GetZ())-(b.GetZ()-a.GetZ())*(a.GetX()-c.GetX()));
float u1 = num1/den;
float u2 = num2/den;
if (den == 0 && num1 == 0 && num2 == 0)
/* The two lines are coincidents */
return false;
if (den == 0)
/* The two lines are parallel */
return false;
if (u1 <0 || u1 > 1 || u2 < 0 || u2 > 1)
/* Lines do not collide */
return false;
/* Lines DO collide */
return true;
}
// PPG Event ==================================================
XSIPLUGINCALLBACK CStatus sn_null2surface_op_PPGEvent( const CRef& in_ctxt )
{
PPGEventContext ctxt( in_ctxt ) ;
PPGEventContext::PPGEvent eventID = ctxt.GetEventID() ;
if ( eventID == PPGEventContext::siOnInit )
{
CRefArray props = ctxt.GetInspectedObjects();
for (LONG i=0; i<props.GetCount( ); i++)
{
// we don't do anything, but I keep the code around
CustomOperator prop( props[i] );
}
}
else if ( eventID == PPGEventContext::siOnClosed )
{
CRefArray props = ctxt.GetInspectedObjects();
for (LONG i=0; i<props.GetCount( ); i++)
{
// we don't do anything, but I keep the code around
CustomOperator prop( props[i] );
}
}
else if ( eventID == PPGEventContext::siButtonClicked )
{
CValue buttonPressed = ctxt.GetAttribute( L"Button" ) ;
if(buttonPressed.GetAsText() == L"setInputUV")
{
CRefArray ops = ctxt.GetInspectedObjects();
for (LONG i=0; i<ops.GetCount( ); i++)
{
CustomOperator op( ops[i] );
CRef driverRef = InputPort(op.GetInputPorts()[0]).GetTarget();
KinematicState driverState(driverRef);
CVector3 driverPos = driverState.GetTransform().GetTranslation();
CRef surfacePrimRef = InputPort(op.GetInputPorts()[2]).GetTarget();
NurbsSurfaceMesh surfaceMesh(Primitive(surfacePrimRef).GetGeometry());
LONG surface_index;
double surface_u;
double surface_v;
double surface_un;
double surface_vn;
double surface_distance;
CVector3 surface_pos;
CVector3 surface_utan;
CVector3 surface_vtan;
CVector3 surface_normal;
surfaceMesh.GetClosestSurfacePosition(driverPos,surface_index,surface_distance,surface_un,surface_vn,surface_pos);
NurbsSurface surface(surfaceMesh.GetSurfaces()[surface_index]);
surface.GetUVFromNormalizedUV(surface_un,surface_vn,surface_u,surface_v);
op.PutParameterValue(L"iu_center",surface_u);
op.PutParameterValue(L"iv_center",surface_v);
}
}
else if(buttonPressed.GetAsText() == L"setInputXY")
{
CRefArray ops = ctxt.GetInspectedObjects();
for (LONG i=0; i<ops.GetCount( ); i++)
{
CustomOperator op( ops[i] );
CRef driverRef = InputPort(op.GetInputPorts()[4]).GetTarget();
KinematicState driverState(driverRef);
CVector3 driverPos = driverState.GetTransform().GetTranslation();
// choose the right values based on the axis_mode
long axis_mode = (LONG)op.GetParameterValue(L"axis_mode");
double surface_u = 0;
double surface_v = 0;
switch(axis_mode)
{
case 0: // X Y plane
{
surface_u = driverPos.GetX();
surface_v = driverPos.GetY();
break;
}
case 1: // X Z plane
{
surface_u = driverPos.GetX();
surface_v = driverPos.GetZ();
break;
}
case 2: // Y Z plane
{
surface_u = driverPos.GetY();
surface_v = driverPos.GetZ();
break;
}
case 3: // Y X plane
{
surface_u = driverPos.GetY();
surface_v = driverPos.GetX();
break;
}
case 4: // Z X plane
{
surface_u = driverPos.GetZ();
surface_v = driverPos.GetX();
break;
}
case 5: // Z Y plane
{
surface_u = driverPos.GetZ();
surface_v = driverPos.GetY();
break;
}
}
op.PutParameterValue(L"ix_center",surface_u);
op.PutParameterValue(L"iy_center",surface_v);
}
}
else if(buttonPressed.GetAsText() == L"setOutputUV")
{
CRefArray ops = ctxt.GetInspectedObjects();
for (LONG i=0; i<ops.GetCount( ); i++)
{
CustomOperator op( ops[i] );
CRef driverRef = InputPort(op.GetInputPorts()[0]).GetTarget();
KinematicState driverState(driverRef);
CVector3 driverPos = driverState.GetTransform().GetTranslation();
CRef surfacePrimRef = InputPort(op.GetInputPorts()[2]).GetTarget();
NurbsSurfaceMesh surfaceMesh(Primitive(surfacePrimRef).GetGeometry());
LONG surface_index;
double surface_u;
double surface_v;
double surface_un;
double surface_vn;
double surface_distance;
CVector3 surface_pos;
CVector3 surface_utan;
CVector3 surface_vtan;
CVector3 surface_normal;
surfaceMesh.GetClosestSurfacePosition(driverPos,surface_index,surface_distance,surface_un,surface_vn,surface_pos);
NurbsSurface surface(surfaceMesh.GetSurfaces()[surface_index]);
surface.GetUVFromNormalizedUV(surface_un,surface_vn,surface_u,surface_v);
op.PutParameterValue(L"ou_center",surface_u);
op.PutParameterValue(L"ov_center",surface_v);
}
}
}
else if ( eventID == PPGEventContext::siTabChange )
{
CRefArray props = ctxt.GetInspectedObjects();
for (LONG i=0; i<props.GetCount( ); i++)
{
// we don't do anything, but I keep the code around
CustomOperator prop( props[i] );
}
}
else if ( eventID == PPGEventContext::siParameterChange )
{
CRefArray props = ctxt.GetInspectedObjects();
for (LONG i=0; i<props.GetCount( ); i++)
{
// we don't do anything, but I keep the code around
CustomOperator prop( props[i] );
}
}
return CStatus::OK ;
}
