FloatRect Transform::transformRect(const FloatRect& rectangle) const
{
// Transform the 4 corners of the rectangle
const Vector2f points[] =
{
transformPoint(rectangle.left, rectangle.top),
transformPoint(rectangle.left, rectangle.top + rectangle.height),
transformPoint(rectangle.left + rectangle.width, rectangle.top),
transformPoint(rectangle.left + rectangle.width, rectangle.top + rectangle.height)
};
// Compute the bounding rectangle of the transformed points
float left = points[0].x;
float top = points[0].y;
float right = points[0].x;
float bottom = points[0].y;
for (int i = 1; i < 4; ++i)
{
if (points[i].x < left) left = points[i].x;
else if (points[i].x > right) right = points[i].x;
if (points[i].y < top) top = points[i].y;
else if (points[i].y > bottom) bottom = points[i].y;
}
return FloatRect(left, top, right - left, bottom - top);
}
Rect32 TransformTools::newRect(const Rect32 &oldRect, const TransformStruct &transform, Point32 *newHotspot) {
Point32 nw(oldRect.left, oldRect.top);
Point32 ne(oldRect.right, oldRect.top);
Point32 sw(oldRect.left, oldRect.bottom);
Point32 se(oldRect.right, oldRect.bottom);
FloatPoint nw1, ne1, sw1, se1;
nw1 = transformPoint(nw - transform._hotspot, transform._angle, transform._zoom);
ne1 = transformPoint(ne - transform._hotspot, transform._angle, transform._zoom);
sw1 = transformPoint(sw - transform._hotspot, transform._angle, transform._zoom);
se1 = transformPoint(se - transform._hotspot, transform._angle, transform._zoom);
float top = MIN(nw1.y, MIN(ne1.y, MIN(sw1.y, se1.y)));
float bottom = MAX(nw1.y, MAX(ne1.y, MAX(sw1.y, se1.y)));
float left = MIN(nw1.x, MIN(ne1.x, MIN(sw1.x, se1.x)));
float right = MAX(nw1.x, MAX(ne1.x, MAX(sw1.x, se1.x)));
if (newHotspot) {
newHotspot->y = (uint32)(-floor(top));
newHotspot->x = (uint32)(-floor(left));
}
Rect32 res;
res.top = (int32)(floor(top)) + transform._hotspot.y;
res.bottom = (int32)(ceil(bottom)) + transform._hotspot.y;
res.left = (int32)(floor(left)) + transform._hotspot.x;
res.right = (int32)(ceil(right)) + transform._hotspot.x;
return res;
}
void MotionEvent::transform(const float matrix[9]) {
// The tricky part of this implementation is to preserve the value of
// rawX and rawY. So we apply the transformation to the first point
// then derive an appropriate new X/Y offset that will preserve rawX
// and rawY for that point.
float oldXOffset = mXOffset;
float oldYOffset = mYOffset;
float newX, newY;
float rawX = getRawX(0);
float rawY = getRawY(0);
transformPoint(matrix, rawX + oldXOffset, rawY + oldYOffset, &newX, &newY);
mXOffset = newX - rawX;
mYOffset = newY - rawY;
// Determine how the origin is transformed by the matrix so that we
// can transform orientation vectors.
float originX, originY;
transformPoint(matrix, 0, 0, &originX, &originY);
// Apply the transformation to all samples.
size_t numSamples = mSamplePointerCoords.size();
for (size_t i = 0; i < numSamples; i++) {
PointerCoords& c = mSamplePointerCoords.editItemAt(i);
float x = c.getAxisValue(AMOTION_EVENT_AXIS_X) + oldXOffset;
float y = c.getAxisValue(AMOTION_EVENT_AXIS_Y) + oldYOffset;
transformPoint(matrix, x, y, &x, &y);
c.setAxisValue(AMOTION_EVENT_AXIS_X, x - mXOffset);
c.setAxisValue(AMOTION_EVENT_AXIS_Y, y - mYOffset);
float orientation = c.getAxisValue(AMOTION_EVENT_AXIS_ORIENTATION);
c.setAxisValue(AMOTION_EVENT_AXIS_ORIENTATION,
transformAngle(matrix, orientation, originX, originY));
}
}
Rectf32 transformRect(const Rectf32 &rectangle) const
{
// Transform the 4 corners of the rectangle
const vector2df points[4] =
{
transformPoint(rectangle.leftTop()),
transformPoint({ rectangle.left, rectangle.bottom() }),
transformPoint({ rectangle.right(), rectangle.top }),
transformPoint(rectangle.rightBottom())
};
// Compute the bounding rectangle of the transformed points
f32 left = points[0].x;
f32 top = points[0].y;
f32 right = points[0].x;
f32 bottom = points[0].y;
for (auto i = 1; i < 4; ++i)
{
if (points[i].x < left) left = points[i].x;
else if (points[i].x > right) right = points[i].x;
if (points[i].y < top) top = points[i].y;
else if (points[i].y > bottom) bottom = points[i].y;
}
return Rectf32{ left, top, right - left, bottom - top };
}
void renderer_addCommandStroke( const float2* pPoints, const uint8* pCommands, uint commandCount )
{
StrokeCommand strokeCommand;
createDrawCommand(&strokeCommand);
const float variance=s_renderer.currentVariance;
for(uint i=0u;i<commandCount;++i)
{
uint8 command=*pCommands++;
float2 pos;
switch(command)
{
case DrawCommand_Move:
if(strokeCommand.data.draw.pointCount>0u)
{
computeStrokeNormals(&strokeCommand, 0);
pushStrokeCommand(&strokeCommand);
}
transformPoint(&pos,pPoints++,variance);
createDrawCommand(&strokeCommand);
if(pushStrokePoint(&pos))
{
strokeCommand.data.draw.pointCount++;
}
//SYS_TRACE_DEBUG("m(%f,%f)\n",pos.x,pos.y);
break;
case DrawCommand_Line:
{
transformPoint(&pos,pPoints++,variance);
if(pushStrokePoint(&pos))
{
strokeCommand.data.draw.pointCount++;
}
//SYS_TRACE_DEBUG("l(%f,%f)\n",pos.x,pos.y);
}
break;
case DrawCommand_Curve:
{
float2 p1,p2;
transformPoint(&p1,pPoints++,variance);
transformPoint(&p2,pPoints++,variance);
strokeCommand.data.draw.pointCount += addQuadraticCurvePointsRec(&pos,&p1,&p2);
pos=p2;
//SYS_TRACE_DEBUG("c(%f,%f)\n",pos.x,pos.y);
}
break;
}
}
if(strokeCommand.data.draw.pointCount>0u)
{
computeStrokeNormals(&strokeCommand, 0);
pushStrokeCommand(&strokeCommand);
}
}
void Triangle::getBounds(Vect4* lower, Vect4* upper) {
Vect4 a = transformPoint(M, this->a);
Vect4 b = transformPoint(M, this->b);
Vect4 c = transformPoint(M, this->c);
*lower = *upper = a;
for (int i = 0; i < 3; i++) {
if (b[i] < (*lower)[i]) (*lower)[i] = b[i];
else if (b[i] > (*upper)[i]) (*upper)[i] = b[i];
if (c[i] < (*lower)[i]) (*lower)[i] = c[i];
else if (c[i] > (*upper)[i]) (*upper)[i] = c[i];
}
}
void drawProjectedQuad(float x1, float x2, float x3, float x4, float y1, float y2, float y3, float y4, float z1, float z2, float z3, float z4, int color, int transprency) {
float transformedX1;
float transformedX2;
float transformedX3;
float transformedX4;
float transformedY1;
float transformedY2;
float transformedY3;
float transformedY4;
x1 -= translateX;
x2 -= translateX;
x3 -= translateX;
x4 -= translateX;
y1 -= translateY;
y2 -= translateY;
y3 -= translateY;
y4 -= translateY;
z1 -= translateZ;
z2 -= translateZ;
z3 -= translateZ;
z4 -= translateZ;
transformPoint(&x1, &y1, &z1);
transformPoint(&x2, &y2, &z2);
transformPoint(&x3, &y3, &z3);
transformPoint(&x4, &y4, &z4);
z1 += cameraX;
z2 += cameraX;
z3 += cameraX;
z4 += cameraX;
transformedX1 = ((x1 * cameraY) / (float)z1) + cameraCenterX;
transformedX2 = ((x2 * cameraY) / (float)z2) + cameraCenterX;
transformedX3 = ((x3 * cameraY) / (float)z3) + cameraCenterX;
transformedX4 = ((x4 * cameraY) / (float)z4) + cameraCenterX;
transformedY1 = ((y1 * cameraZ) / (float)z1) + cameraCenterY;
transformedY2 = ((y2 * cameraZ) / (float)z2) + cameraCenterY;
transformedY3 = ((y3 * cameraZ) / (float)z3) + cameraCenterY;
transformedY4 = ((y4 * cameraZ) / (float)z4) + cameraCenterY;
if(z1 > DEPTH_THRESHOLD && z2 > DEPTH_THRESHOLD && z3 > DEPTH_THRESHOLD && z4 > DEPTH_THRESHOLD) {
g_driver->draw3dQuad(transformedX1, transformedY1, z1, transformedX2, transformedY2, z2, transformedX3, transformedY3, z3, transformedX4, transformedY4, z4, color, transprency);
}
//g_driver->draw3dQuad(x1,y1,z1, x2,y2,z2, x3,y3,z3, x4,y4,z4, color);
}
void Physics3DConstraintDemo::onTouchesBegan(const std::vector<cocos2d::Touch*>& touches, cocos2d::Event *event)
{
//ray trace
if(_camera)
{
auto touch = touches[0];
auto location = touch->getLocationInView();
Vec3 nearP(location.x, location.y, 0.0f), farP(location.x, location.y, 1.0f);
auto size = Director::getInstance()->getWinSize();
_camera->unproject(size, &nearP, &nearP);
_camera->unproject(size, &farP, &farP);
Physics3DWorld::HitResult result;
bool ret = physicsScene->getPhysics3DWorld()->rayCast(nearP, farP, &result);
if (ret && result.hitObj->getObjType() == Physics3DObject::PhysicsObjType::RIGID_BODY)
{
auto mat = result.hitObj->getWorldTransform().getInversed();
Vec3 position;
mat.transformPoint(result.hitPosition, &position);
_constraint = Physics3DPointToPointConstraint::create(static_cast<Physics3DRigidBody*>(result.hitObj), position);
physicsScene->getPhysics3DWorld()->addPhysics3DConstraint(_constraint, true);
_pickingDistance = (result.hitPosition - nearP).length();
return;
}
}
Physics3DTestDemo::onTouchesBegan(touches, event);
_needShootBox = false;
}
static void renderer_addSingleStroke( const float2* pStrokePoints, uint strokePointCount )
{
SYS_ASSERT( s_renderer.pageState == PageState_BeforeDraw );
if( !pStrokePoints || strokePointCount < 2u )
{
return;
}
const int isCycle=float2_isEqual(&pStrokePoints[0u],&pStrokePoints[strokePointCount-1u]);
StrokeCommand command;
createDrawCommand( &command );
const float variance = s_renderer.currentVariance;
// transform, randomize and copy positions
for( uint i = 0u; i < strokePointCount; ++i )
{
const float2 strokePoint = pStrokePoints[ i ];
// reduced variance in the beginning
const int isFirstVertexInStroke = command.data.draw.pointCount == 0u;
float2 point;
transformPoint( &point, &strokePoint, isFirstVertexInStroke ? variance / 4.0f : variance );
if( pushStrokePoint( &point ) )
{
command.data.draw.pointCount++;
}
}
computeStrokeNormals( &command, isCycle );
pushStrokeCommand( &command );
}
void CadItem::transformPoints()
{
points.clear();
foreach(QPointF point, pointPolygon)
{
points << transformPoint(point);
}
void FaceDetectorFilter::facesCallback(const pcl::PointCloud<pcl::PointXYZL>::ConstPtr& msg)
{
tf::Transform cameraTransform;
try {
tf::StampedTransform tr;
transformListener.lookupTransform ( "odom","camera_link2",ros::Time(0),tr);
cameraTransform=tr;
} catch(...) {
return;
}
std::set<unsigned int> usedUsers = deleteOld();
std::list<Point> incomingUsers;
fillList(incomingUsers, msg,cameraTransform);
while(1) {
std::pair<unsigned int,std::list<Point>::iterator> match = findClosest(incomingUsers);
if(match.first == 0)
break;
float distance = getDistance(users[match.first],*match.second);
if(distance> MAX_SPEED)
break;
if(usedUsers.find(match.first) == usedUsers.end()) {
users[match.first] = *match.second;
usedUsers.insert(match.first);
std::cerr<<"user updated: "<<match.first<<", distance:" <<distance<<std::endl;
} else {
std::cerr<<"user ignored: "<<match.first<<", distance:" <<distance<<std::endl;
}
incomingUsers.erase(match.second);
}
for(std::list<Point>::iterator it = incomingUsers.begin(); it!=incomingUsers.end(); ++it) {
unsigned int newId = getAvailableId(usedUsers);
users[newId] = *it;
std::cerr<<"added user: "******"camera_link2";
pmsg->height = 1;
for(std::map<unsigned int,Point>::iterator it = users.begin(); it != users.end(); ++it) {
pcl::PointXYZL point;
Point p(it->second);
transformPoint(p,cameraTransform,true);
point.label = it->first;
point.x=p.x;
point.y=p.y;
point.z = p.z;
pmsg->points.push_back(point);
}
pmsg->width = pmsg->points.size();
facePublisher.publish(pmsg);
}
void Hero::_armEventHandler(cocos2d::EventCustom* event)
{
const auto eventObject = (dragonBones::EventObject*)event->getUserData();
if (eventObject->type == dragonBones::EventObject::COMPLETE)
{
_isAttacking = false;
_hitCount = 0;
const auto animationName = "ready_" + _weaponName;
_armArmature->getAnimation().fadeIn(animationName);
}
else if (eventObject->type == dragonBones::EventObject::FRAME_EVENT)
{
if (eventObject->name == "ready")
{
_isAttacking = false;
_hitCount++;
}
else if (eventObject->name == "fire")
{
const auto display = (dragonBones::CCArmatureDisplay*)(eventObject->armature->getDisplay());
const auto firePointBone = eventObject->armature->getBone("bow");
const auto transform = display->getNodeToWorldTransform();
cocos2d::Vec3 localPoint(firePointBone->global.x, -firePointBone->global.y, 0.f);
cocos2d::Vec2 globalPoint;
transform.transformPoint(&localPoint);
globalPoint.set(localPoint.x, localPoint.y);
auto radian = 0.f;
if (_faceDir > 0)
{
radian = firePointBone->global.getRotation() + display->getRotation() * dragonBones::ANGLE_TO_RADIAN;
}
else
{
radian = dragonBones::PI - (firePointBone->global.getRotation() + display->getRotation() * dragonBones::ANGLE_TO_RADIAN);
}
switch (_weaponsLevel[_weaponIndex])
{
case 0:
_fire(globalPoint, radian);
break;
case 1:
_fire(globalPoint, radian + 3.f * dragonBones::ANGLE_TO_RADIAN);
_fire(globalPoint, radian - 3.f * dragonBones::ANGLE_TO_RADIAN);
break;
case 2:
_fire(globalPoint, radian + 6.f * dragonBones::ANGLE_TO_RADIAN);
_fire(globalPoint, radian);
_fire(globalPoint, radian - 6.f * dragonBones::ANGLE_TO_RADIAN);
break;
}
}
}
}
void LeapMotionPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,
const std::vector<LeapMotionPlugin::LeapMotionJoint>& joints,
const std::vector<LeapMotionPlugin::LeapMotionJoint>& prevJoints) {
glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
glm::quat controllerToAvatarRotation = glmExtractRotation(controllerToAvatar);
glm::vec3 hmdSensorPosition; // HMD
glm::quat hmdSensorOrientation; // HMD
glm::vec3 leapMotionOffset; // Desktop
if (_isLeapOnHMD) {
hmdSensorPosition = extractTranslation(inputCalibrationData.hmdSensorMat);
hmdSensorOrientation = extractRotation(inputCalibrationData.hmdSensorMat);
} else {
// Desktop "zero" position is some distance above the Leap Motion sensor and half the avatar's shoulder-to-hand length
// in front of avatar.
float halfShouldToHandLength = fabsf(extractTranslation(inputCalibrationData.defaultLeftHand).x
- extractTranslation(inputCalibrationData.defaultLeftArm).x) / 2.0f;
leapMotionOffset = glm::vec3(0.0f, _desktopHeightOffset, halfShouldToHandLength);
}
for (size_t i = 0; i < joints.size(); i++) {
int poseIndex = LeapMotionJointIndexToPoseIndex((LeapMotionJointIndex)i);
if (joints[i].position == Vectors::ZERO) {
_poseStateMap[poseIndex] = controller::Pose();
continue;
}
glm::vec3 pos;
glm::quat rot;
if (_isLeapOnHMD) {
auto jointPosition = joints[i].position;
const glm::vec3 HMD_EYE_TO_LEAP_OFFSET = glm::vec3(0.0f, 0.0f, -0.09f); // Eyes to surface of Leap Motion.
jointPosition = glm::vec3(-jointPosition.x, -jointPosition.z, -jointPosition.y) + HMD_EYE_TO_LEAP_OFFSET;
jointPosition = hmdSensorPosition + hmdSensorOrientation * jointPosition;
pos = transformPoint(controllerToAvatar, jointPosition);
glm::quat jointOrientation = joints[i].orientation;
jointOrientation = glm::quat(jointOrientation.w, -jointOrientation.x, -jointOrientation.z, -jointOrientation.y);
rot = controllerToAvatarRotation * hmdSensorOrientation * jointOrientation;
} else {
pos = controllerToAvatarRotation * (joints[i].position - leapMotionOffset);
const glm::quat ZERO_HAND_ORIENTATION = glm::quat(glm::vec3(PI_OVER_TWO, PI, 0.0f));
rot = controllerToAvatarRotation * joints[i].orientation * ZERO_HAND_ORIENTATION;
}
glm::vec3 linearVelocity, angularVelocity;
if (i < prevJoints.size()) {
linearVelocity = (pos - (prevJoints[i].position * METERS_PER_CENTIMETER)) / deltaTime; // m/s
// quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation.
glm::quat d = glm::log(rot * glm::inverse(prevJoints[i].orientation));
angularVelocity = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s
}
_poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVelocity, angularVelocity);
}
}
Pose Pose::transform(const glm::mat4& mat) const {
auto rot = glmExtractRotation(mat);
Pose pose(transformPoint(mat, translation),
rot * rotation,
transformVectorFast(mat, velocity),
rot * angularVelocity);
pose.valid = valid;
return pose;
}
void renderer_addBurnHole( const float2* pStart, const float2* pEnd, float size )
{
for( uint i = 0u; i < SYS_COUNTOF(s_renderer.burnHoles); ++i )
{
if( s_renderer.burnHoles[i].size <= 0.0f )
{
s_renderer.burnHoles[i].size=size;
s_renderer.burnHoles[i].initialSize=size;
transformPoint( &s_renderer.burnHoles[i].start, pStart, 0.0f );
transformPoint( &s_renderer.burnHoles[i].end, pEnd, 0.0f );
s_renderer.burnHoles[i].rot=float_rand_range(0.0f, 2.0f*PI);
drawBurnHole( &s_renderer.burnHoles[i] );
return;
}
}
SYS_TRACE_WARNING( "no burn hole slot found!\n" );
}
Ray Matrix4x4::transformRay(const Ray &r) const
{
Ray transformedRay = r;
Vector3D transformedOrigin = transformPoint(r.o);
Vector3D transformedDir = transformVector(r.d);
transformedRay.o = transformedOrigin;
transformedRay.d = transformedDir;
return transformedRay;
}
void Homography::setSrcRect( const Rectf & r )
{
srcPts[0](r.l,r.t);
srcPts[1](r.r,r.t);
srcPts[2](r.r,r.b);
srcPts[3](r.l,r.b);
for( int i = 0; i < 4; i++ )
{
dstPts[i] = transformPoint(srcPts[i]);
}
compute();
}
sf::ConvexShape convertRectToConvex(const Shape& base, const VanishPlane& plane)
{
auto transform = base.getTransform();
const unsigned n_points = base.getPointCount();
sf::ConvexShape result(n_points);
for(unsigned i = 0; i < n_points; ++i)
result.setPoint(i, plane(transform.transformPoint(base.getPoint(i))));
result.setFillColor(base.getFillColor());
result.setOutlineColor(base.getOutlineColor());
result.setOutlineThickness(1);
return result;
}
OBB::OBB(const AABB& aabb)
{
corner[0] = vec2(aabb.x1,aabb.y1);
corner[1] = vec2(aabb.x2,aabb.y1);
corner[2] = vec2(aabb.x2,aabb.y2);
corner[3] = vec2(aabb.x1,aabb.y2);
// rotate by current matrix
mat4 mt = glGetCurrentMatrix(GL_MODELVIEW_MATRIX);
for (int c = 0; c < 4; ++c)
corner[c] = vec2(transformPoint(mt,vec3(corner[c])));
computeAxes();
}
void Rat::update(float dt)
{
switch (_state)
{
case RAT_IDLE:
break;
case RAT_FORWARD:
{
Vec3 curPos = this->getPosition3D();
Vec3 newFaceDir = _targetPos - curPos;//新的脸朝向为目的坐标-当前坐标，从场景的OnTouchEnded获得
newFaceDir.y = 0.0f;//只考虑xz平面上脸朝向的变化
newFaceDir.normalize();
Vec3 offset = newFaceDir*RAT_FORWARD_SPEED*dt;//向脸的朝向每秒位移n个单位
curPos = curPos + offset;
this->setPosition3D(curPos);
}
break;
case RAT_KNOCKED:
break;
case RAT_ATTACK:
break;
case RAT_DEAD:
break;
default:
break;
}
// transform player position to world coord
auto playerPos = this->getPosition3D();//获得人物在父节点(层)中的空间三维坐标
auto playerModelMat = this->getParent()->getNodeToWorldTransform();//得到人物所在父节点(层)的坐标向世界坐标转换的矩阵
playerModelMat.transformPoint(&playerPos);//对人物所在点进行转换
Vec3 Normal;
float player_h = 0;//根据角色的x、z坐标获得高度
if (Normal.isZero())//check the player whether is out of the terrain
{
player_h = playerPos.y;
}
else
{
player_h += PLAYER_HEIGHT;
}
this->setPositionY(player_h);
Quaternion q2;
q2.createFromAxisAngle(Vec3(0, 1, 0), (float)-M_PI, &q2);//此四元数表示绕y轴转过180度，即令角色转身
Quaternion headingQ;
headingQ.createFromAxisAngle(_headingAxis, _headingAngle, &headingQ);
this->setRotationQuat(headingQ*q2);
this->updateState();
}
xmlNodePtr nodeElement(NODE node)
{
if (strcmp(srid, "4326") != 0)
{
node = transformPoint(node, srid);
}
xmlNodePtr osmNode;
osmNode = xmlNewNode(NULL, BAD_CAST "node");
xmlNewProp(osmNode, BAD_CAST "id", BAD_CAST xmlEscape(int2char(node.id)));
xmlNewProp(osmNode, BAD_CAST "lon", BAD_CAST xmlEscape(dbl2char(node.x)));
xmlNewProp(osmNode, BAD_CAST "lat", BAD_CAST xmlEscape(dbl2char(node.y)));
return osmNode;
}
TGLint tgluProject(TGLfloat objx, TGLfloat objy, TGLfloat objz, const TGLfloat model[16], const TGLfloat proj[16],
const TGLint viewport[4], TGLfloat *winx, TGLfloat *winy, TGLfloat *winz) {
TGLfloat in[4], out[4];
in[0] = objx;
in[1] = objy;
in[2] = objz;
in[3] = 1.0f;
transformPoint(out, model, in);
transformPoint(in, proj, out);
if (in[3] == 0.0)
return TGL_FALSE;
in[0] /= in[3];
in[1] /= in[3];
in[2] /= in[3];
*winx = viewport[0] + (1 + in[0]) * viewport[2] / 2;
*winy = viewport[1] + (1 + in[1]) * viewport[3] / 2;
*winz = (1 + in[2]) / 2;
return TGL_TRUE;
}
void Mecha::_frameEventHandler(cocos2d::EventCustom* event)
{
const auto eventObject = (dragonBones::EventObject*)event->getUserData();
if (eventObject->name == "onFire")
{
const auto display = (dragonBones::CCArmatureDisplay*)eventObject->armature->getDisplay();
const auto firePointBone = eventObject->armature->getBone("firePoint");
const auto transform = display->getNodeToWorldTransform();
cocos2d::Vec3 localPoint(firePointBone->global.x, -firePointBone->global.y, 0.f);
cocos2d::Vec2 globalPoint;
transform.transformPoint(&localPoint);
globalPoint.set(localPoint.x, localPoint.y);
_fire(globalPoint);
}
}
// Logic based on http://clb.demon.fi/MathGeoLib/nightly/docs/AABB.cpp_code.html#471
void AABox::transform(const glm::mat4& matrix) {
// FIXME use simd operations
auto halfSize = _scale * 0.5f;
auto center = _corner + halfSize;
halfSize = abs(halfSize);
auto mm = glm::transpose(glm::mat3(matrix));
vec3 newDir = vec3(
glm::dot(glm::abs(mm[0]), halfSize),
glm::dot(glm::abs(mm[1]), halfSize),
glm::dot(glm::abs(mm[2]), halfSize)
);
auto newCenter = transformPoint(matrix, center);
_corner = newCenter - newDir;
_scale = newDir * 2.0f;
}
int FaceDetectorFilter::fillList(std::list<Point> & list,pcl::PointCloud<pcl::PointXYZL>::ConstPtr msg,const tf::Transform cameraTransform)
{
ros::Time now = ros::Time::now();
for(pcl::PointCloud<pcl::PointXYZL>::const_iterator it= msg->points.begin(); it!= msg->points.end(); ++it) {
Point p;
p.x = it->x;
p.y = it->y;
p.z = it->z;
p.lastSeen = now;
transformPoint(p,cameraTransform,false);
list.push_back(p);
}
return msg->points.size();
}
static float transformAngle(const float matrix[9], float angleRadians,
float originX, float originY) {
// Construct and transform a vector oriented at the specified clockwise angle from vertical.
// Coordinate system: down is increasing Y, right is increasing X.
float x = sinf(angleRadians);
float y = -cosf(angleRadians);
transformPoint(matrix, x, y, &x, &y);
x -= originX;
y -= originY;
// Derive the transformed vector's clockwise angle from vertical.
float result = atan2f(x, -y);
if (result < - M_PI_2) {
result += M_PI;
} else if (result > M_PI_2) {
result -= M_PI;
}
return result;
}
geometry_msgs::Pose transformPose(geometry_msgs::Pose pose_in, Eigen::Matrix4f transf){
geometry_msgs::Pose pose_out;
// Obtener rotación desde matriz de transformación
Eigen::Matrix4f rot;
rot = transf;
rot.col(3) = Eigen::Vector4f(0, 0, 0, 1);
// Crear normal desde quaternion
tf::Quaternion tf_q;
tf::quaternionMsgToTF(pose_in.orientation, tf_q);
tf::Vector3 normal(1, 0, 0);
normal = tf::quatRotate(tf_q, normal);
normal.normalize();
// Rotar normal
Eigen::Vector3f normal_vector (normal.x(), normal.y(), normal.z());
Eigen::Vector3f normal_rotated = transformVector(normal_vector, transf);
normal_rotated.normalize();
// Obtener quaternion desde normal rotada
pose_out.orientation = coefsToQuaternionMsg(normal_rotated[0], normal_rotated[1], normal_rotated[2]);
// Transportar posición
pose_out.position = transformPoint(pose_in.position, transf);
return pose_out;
}
void TransformTests::getInverseMatrix() {
const vec3 t(0.0f, 0.0f, 10.0f);
// create a matrix that is composed of a PI/2 rotation followed by a small z translation
const mat4 m(vec4(rot90 * xAxis, 0.0f),
vec4(rot90 * yAxis, 0.0f),
vec4(rot90 * zAxis, 0.0f),
vec4(vec4(t, 1.0f)));
// mirror about the x axis.
const mat4 mirrorX(vec4(-1.0f, 0.0f, 0.0f, 0.0f),
vec4( 0.0f, 1.0f, 0.0f, 0.0f),
vec4( 0.0f, 0.0f, 1.0f, 0.0f),
vec4( 0.0f, 0.0f, 0.0f, 1.0f));
const mat4 result_a = glm::inverse(m * mirrorX);
Transform xform;
xform.setTranslation(t);
xform.setRotation(rot90);
xform.postScale(vec3(-1.0f, 1.0f, 1.0f));
mat4 result_b;
xform.getInverseMatrix(result_b);
// don't check elements directly, instead compare each axis transformed by the matrix.
auto xa = transformPoint(result_a, xAxis);
auto ya = transformPoint(result_a, yAxis);
auto za = transformPoint(result_a, zAxis);
auto xb = transformPoint(result_b, xAxis);
auto yb = transformPoint(result_b, yAxis);
auto zb = transformPoint(result_b, zAxis);
QCOMPARE_WITH_ABS_ERROR(xa, xb, TEST_EPSILON);
QCOMPARE_WITH_ABS_ERROR(ya, yb, TEST_EPSILON);
QCOMPARE_WITH_ABS_ERROR(za, zb, TEST_EPSILON);
}
Vector PerspectiveCamera::rasterToWorld(const Vector &rsP, const PTime time) const
{
return transformPoint(rsP, m_rasterToWorld, time);
}
Vector PerspectiveCamera::worldToRaster(const Vector &wsP, const PTime time) const
{
return transformPoint(wsP, m_worldToRaster, time);
}