bool KX_RaySensor::Evaluate()
{
bool result = false;
bool reset = m_reset && m_level;
m_rayHit = false;
m_hitObject = NULL;
m_hitPosition[0] = 0;
m_hitPosition[1] = 0;
m_hitPosition[2] = 0;
m_hitNormal[0] = 1;
m_hitNormal[1] = 0;
m_hitNormal[2] = 0;
KX_GameObject* obj = (KX_GameObject*)GetParent();
MT_Point3 frompoint = obj->NodeGetWorldPosition();
MT_Matrix3x3 matje = obj->NodeGetWorldOrientation();
MT_Matrix3x3 invmat = matje.inverse();
MT_Vector3 todir;
m_reset = false;
switch (m_axis)
{
case SENS_RAY_X_AXIS: // X
{
todir[0] = invmat[0][0];
todir[1] = invmat[0][1];
todir[2] = invmat[0][2];
break;
}
case SENS_RAY_Y_AXIS: // Y
{
todir[0] = invmat[1][0];
todir[1] = invmat[1][1];
todir[2] = invmat[1][2];
break;
}
case SENS_RAY_Z_AXIS: // Z
{
todir[0] = invmat[2][0];
todir[1] = invmat[2][1];
todir[2] = invmat[2][2];
break;
}
case SENS_RAY_NEG_X_AXIS: // -X
{
todir[0] = -invmat[0][0];
todir[1] = -invmat[0][1];
todir[2] = -invmat[0][2];
break;
}
case SENS_RAY_NEG_Y_AXIS: // -Y
{
todir[0] = -invmat[1][0];
todir[1] = -invmat[1][1];
todir[2] = -invmat[1][2];
break;
}
case SENS_RAY_NEG_Z_AXIS: // -Z
{
todir[0] = -invmat[2][0];
todir[1] = -invmat[2][1];
todir[2] = -invmat[2][2];
break;
}
}
todir.normalize();
m_rayDirection[0] = todir[0];
m_rayDirection[1] = todir[1];
m_rayDirection[2] = todir[2];
MT_Point3 topoint = frompoint + (m_distance) * todir;
PHY_IPhysicsEnvironment* pe = m_scene->GetPhysicsEnvironment();
if (!pe)
{
std::cout << "WARNING: Ray sensor " << GetName() << ": There is no physics environment!" << std::endl;
std::cout << " Check universe for malfunction." << std::endl;
return false;
}
KX_IPhysicsController *spc = obj->GetPhysicsController();
KX_GameObject *parent = obj->GetParent();
if (!spc && parent)
spc = parent->GetPhysicsController();
if (parent)
parent->Release();
PHY_IPhysicsEnvironment* physics_environment = this->m_scene->GetPhysicsEnvironment();
KX_RayCast::Callback<KX_RaySensor> callback(this, spc);
KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback);
/* now pass this result to some controller */
if (m_rayHit)
{
if (!m_bTriggered)
{
// notify logicsystem that ray is now hitting
result = true;
m_bTriggered = true;
}
else
{
// notify logicsystem that ray is STILL hitting ...
result = false;
}
}
else
{
if (m_bTriggered)
{
m_bTriggered = false;
// notify logicsystem that ray JUST left the Object
result = true;
}
else
{
result = false;
}
}
if (reset)
// force an event
result = true;
return result;
}
void RAS_OpenGLRasterizer::applyTransform(double* oglmatrix,int objectdrawmode )
{
/* FIXME:
blender: intern/moto/include/MT_Vector3.inl:42: MT_Vector3 operator/(const
MT_Vector3&, double): Assertion `!MT_fuzzyZero(s)' failed.
Program received signal SIGABRT, Aborted.
[Switching to Thread 16384 (LWP 1519)]
0x40477571 in kill () from /lib/libc.so.6
(gdb) bt
#7 0x08334368 in MT_Vector3::normalized() const ()
#8 0x0833e6ec in RAS_OpenGLRasterizer::applyTransform(RAS_IRasterizer*, double*, int) ()
*/
if (objectdrawmode & RAS_IPolyMaterial::BILLBOARD_SCREENALIGNED ||
objectdrawmode & RAS_IPolyMaterial::BILLBOARD_AXISALIGNED)
{
// rotate the billboard/halo
//page 360/361 3D Game Engine Design, David Eberly for a discussion
// on screen aligned and axis aligned billboards
// assumed is that the preprocessor transformed all billboard polygons
// so that their normal points into the positive x direction (1.0, 0.0, 0.0)
// when new parenting for objects is done, this rotation
// will be moved into the object
MT_Point3 objpos (oglmatrix[12],oglmatrix[13],oglmatrix[14]);
MT_Point3 campos = GetCameraPosition();
MT_Vector3 dir = (campos - objpos).safe_normalized();
MT_Vector3 up(0,0,1.0);
KX_GameObject* gameobj = (KX_GameObject*)m_clientobject;
// get scaling of halo object
MT_Vector3 size = gameobj->GetSGNode()->GetWorldScaling();
bool screenaligned = (objectdrawmode & RAS_IPolyMaterial::BILLBOARD_SCREENALIGNED)!=0;//false; //either screen or axisaligned
if (screenaligned)
{
up = (up - up.dot(dir) * dir).safe_normalized();
} else
{
dir = (dir - up.dot(dir)*up).safe_normalized();
}
MT_Vector3 left = dir.normalized();
dir = (up.cross(left)).normalized();
// we have calculated the row vectors, now we keep
// local scaling into account:
left *= size[0];
dir *= size[1];
up *= size[2];
double maat[16] = {left[0], left[1], left[2], 0,
dir[0], dir[1], dir[2], 0,
up[0], up[1], up[2], 0,
0, 0, 0, 1};
glTranslated(objpos[0],objpos[1],objpos[2]);
glMultMatrixd(maat);
}
else {
if (objectdrawmode & RAS_IPolyMaterial::SHADOW)
{
// shadow must be cast to the ground, physics system needed here!
MT_Point3 frompoint(oglmatrix[12],oglmatrix[13],oglmatrix[14]);
KX_GameObject *gameobj = (KX_GameObject*)m_clientobject;
MT_Vector3 direction = MT_Vector3(0,0,-1);
direction.normalize();
direction *= 100000;
MT_Point3 topoint = frompoint + direction;
KX_Scene* kxscene = (KX_Scene*) m_auxilaryClientInfo;
PHY_IPhysicsEnvironment* physics_environment = kxscene->GetPhysicsEnvironment();
PHY_IPhysicsController* physics_controller = gameobj->GetPhysicsController();
KX_GameObject *parent = gameobj->GetParent();
if (!physics_controller && parent)
physics_controller = parent->GetPhysicsController();
if (parent)
parent->Release();
KX_RayCast::Callback<RAS_OpenGLRasterizer> callback(this, physics_controller, oglmatrix);
if (!KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback))
{
// couldn't find something to cast the shadow on...
glMultMatrixd(oglmatrix);
}
else
{ // we found the "ground", but the cast matrix doesn't take
// scaling in consideration, so we must apply the object scale
MT_Vector3 size = gameobj->GetSGNode()->GetLocalScale();
glScalef(size[0], size[1], size[2]);
}
} else
{
// 'normal' object
glMultMatrixd(oglmatrix);
}
}
}
bool KX_ConstraintActuator::Update(double curtime, bool frame)
{
bool result = false;
bool bNegativeEvent = IsNegativeEvent();
RemoveAllEvents();
if (!bNegativeEvent) {
/* Constraint clamps the values to the specified range, with a sort of */
/* low-pass filtered time response, if the damp time is unequal to 0. */
/* Having to retrieve location/rotation and setting it afterwards may not */
/* be efficient enough... Something to look at later. */
KX_GameObject *obj = (KX_GameObject*) GetParent();
MT_Point3 position = obj->NodeGetWorldPosition();
MT_Point3 newposition;
MT_Vector3 normal, direction, refDirection;
MT_Matrix3x3 rotation = obj->NodeGetWorldOrientation();
MT_Scalar filter, newdistance, cosangle;
int axis, sign;
if (m_posDampTime) {
filter = m_posDampTime/(1.0+m_posDampTime);
} else {
filter = 0.0;
}
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
case KX_ACT_CONSTRAINT_ORIY:
case KX_ACT_CONSTRAINT_ORIZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
direction[0] = rotation[0][0];
direction[1] = rotation[1][0];
direction[2] = rotation[2][0];
axis = 0;
break;
case KX_ACT_CONSTRAINT_ORIY:
direction[0] = rotation[0][1];
direction[1] = rotation[1][1];
direction[2] = rotation[2][1];
axis = 1;
break;
default:
direction[0] = rotation[0][2];
direction[1] = rotation[1][2];
direction[2] = rotation[2][2];
axis = 2;
break;
}
if ((m_maximumBound < (1.0f-FLT_EPSILON)) || (m_minimumBound < (1.0f-FLT_EPSILON))) {
// reference direction needs to be evaluated
// 1. get the cosine between current direction and target
cosangle = direction.dot(m_refDirVector);
if (cosangle >= (m_maximumBound-FLT_EPSILON) && cosangle <= (m_minimumBound+FLT_EPSILON)) {
// no change to do
result = true;
goto CHECK_TIME;
}
// 2. define a new reference direction
// compute local axis with reference direction as X and
// Y in direction X refDirection plane
MT_Vector3 zaxis = m_refDirVector.cross(direction);
if (MT_fuzzyZero2(zaxis.length2())) {
// direction and refDirection are identical,
// choose any other direction to define plane
if (direction[0] < 0.9999)
zaxis = m_refDirVector.cross(MT_Vector3(1.0,0.0,0.0));
else
zaxis = m_refDirVector.cross(MT_Vector3(0.0,1.0,0.0));
}
MT_Vector3 yaxis = zaxis.cross(m_refDirVector);
yaxis.normalize();
if (cosangle > m_minimumBound) {
// angle is too close to reference direction,
// choose a new reference that is exactly at minimum angle
refDirection = m_minimumBound * m_refDirVector + m_minimumSine * yaxis;
} else {
// angle is too large, choose new reference direction at maximum angle
refDirection = m_maximumBound * m_refDirVector + m_maximumSine * yaxis;
}
} else {
refDirection = m_refDirVector;
}
// apply damping on the direction
direction = filter*direction + (1.0-filter)*refDirection;
obj->AlignAxisToVect(direction, axis);
result = true;
goto CHECK_TIME;
case KX_ACT_CONSTRAINT_DIRPX:
case KX_ACT_CONSTRAINT_DIRPY:
case KX_ACT_CONSTRAINT_DIRPZ:
case KX_ACT_CONSTRAINT_DIRNX:
case KX_ACT_CONSTRAINT_DIRNY:
case KX_ACT_CONSTRAINT_DIRNZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_DIRPX:
normal[0] = rotation[0][0];
normal[1] = rotation[1][0];
normal[2] = rotation[2][0];
axis = 0; // axis according to KX_GameObject::AlignAxisToVect()
sign = 0; // X axis will be parrallel to direction of ray
break;
case KX_ACT_CONSTRAINT_DIRPY:
normal[0] = rotation[0][1];
normal[1] = rotation[1][1];
normal[2] = rotation[2][1];
axis = 1;
sign = 0;
break;
case KX_ACT_CONSTRAINT_DIRPZ:
normal[0] = rotation[0][2];
normal[1] = rotation[1][2];
normal[2] = rotation[2][2];
axis = 2;
sign = 0;
break;
case KX_ACT_CONSTRAINT_DIRNX:
normal[0] = -rotation[0][0];
normal[1] = -rotation[1][0];
normal[2] = -rotation[2][0];
axis = 0;
sign = 1;
break;
case KX_ACT_CONSTRAINT_DIRNY:
normal[0] = -rotation[0][1];
normal[1] = -rotation[1][1];
normal[2] = -rotation[2][1];
axis = 1;
sign = 1;
break;
case KX_ACT_CONSTRAINT_DIRNZ:
normal[0] = -rotation[0][2];
normal[1] = -rotation[1][2];
normal[2] = -rotation[2][2];
axis = 2;
sign = 1;
break;
}
normal.normalize();
if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
// direction of the ray is along the local axis
direction = normal;
} else {
switch (m_locrot) {
case KX_ACT_CONSTRAINT_DIRPX:
direction = MT_Vector3(1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRPY:
direction = MT_Vector3(0.0,1.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRPZ:
direction = MT_Vector3(0.0,0.0,1.0);
break;
case KX_ACT_CONSTRAINT_DIRNX:
direction = MT_Vector3(-1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRNY:
direction = MT_Vector3(0.0,-1.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRNZ:
direction = MT_Vector3(0.0,0.0,-1.0);
break;
}
}
{
MT_Point3 topoint = position + (m_maximumBound) * direction;
PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
PHY_IPhysicsController *spc = obj->GetPhysicsController();
if (!pe) {
std::cout << "WARNING: Constraint actuator " << GetName() << ": There is no physics environment!" << std::endl;
goto CHECK_TIME;
}
if (!spc) {
// the object is not physical, we probably want to avoid hitting its own parent
KX_GameObject *parent = obj->GetParent();
if (parent) {
spc = parent->GetPhysicsController();
parent->Release();
}
}
KX_RayCast::Callback<KX_ConstraintActuator> callback(this,dynamic_cast<PHY_IPhysicsController*>(spc));
result = KX_RayCast::RayTest(pe, position, topoint, callback);
if (result) {
MT_Vector3 newnormal = callback.m_hitNormal;
// compute new position & orientation
if ((m_option & (KX_ACT_CONSTRAINT_NORMAL|KX_ACT_CONSTRAINT_DISTANCE)) == 0) {
// if none option is set, the actuator does nothing but detect ray
// (works like a sensor)
goto CHECK_TIME;
}
if (m_option & KX_ACT_CONSTRAINT_NORMAL) {
MT_Scalar rotFilter;
// apply damping on the direction
if (m_rotDampTime) {
rotFilter = m_rotDampTime/(1.0+m_rotDampTime);
} else {
rotFilter = filter;
}
newnormal = rotFilter*normal - (1.0-rotFilter)*newnormal;
obj->AlignAxisToVect((sign)?-newnormal:newnormal, axis);
if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
direction = newnormal;
direction.normalize();
}
}
if (m_option & KX_ACT_CONSTRAINT_DISTANCE) {
if (m_posDampTime) {
newdistance = filter*(position-callback.m_hitPoint).length()+(1.0-filter)*m_minimumBound;
} else {
newdistance = m_minimumBound;
}
// logically we should cancel the speed along the ray direction as we set the
// position along that axis
spc = obj->GetPhysicsController();
if (spc && spc->IsDynamic()) {
MT_Vector3 linV = spc->GetLinearVelocity();
// cancel the projection along the ray direction
MT_Scalar fallspeed = linV.dot(direction);
if (!MT_fuzzyZero(fallspeed))
spc->SetLinearVelocity(linV-fallspeed*direction,false);
}
} else {
newdistance = (position-callback.m_hitPoint).length();
}
newposition = callback.m_hitPoint-newdistance*direction;
} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
// no contact but still keep running
result = true;
goto CHECK_TIME;
}
}
break;
case KX_ACT_CONSTRAINT_FHPX:
case KX_ACT_CONSTRAINT_FHPY:
case KX_ACT_CONSTRAINT_FHPZ:
case KX_ACT_CONSTRAINT_FHNX:
case KX_ACT_CONSTRAINT_FHNY:
case KX_ACT_CONSTRAINT_FHNZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_FHPX:
normal[0] = -rotation[0][0];
normal[1] = -rotation[1][0];
normal[2] = -rotation[2][0];
direction = MT_Vector3(1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHPY:
normal[0] = -rotation[0][1];
normal[1] = -rotation[1][1];
normal[2] = -rotation[2][1];
direction = MT_Vector3(0.0,1.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHPZ:
normal[0] = -rotation[0][2];
normal[1] = -rotation[1][2];
normal[2] = -rotation[2][2];
direction = MT_Vector3(0.0,0.0,1.0);
break;
case KX_ACT_CONSTRAINT_FHNX:
normal[0] = rotation[0][0];
normal[1] = rotation[1][0];
normal[2] = rotation[2][0];
direction = MT_Vector3(-1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHNY:
normal[0] = rotation[0][1];
normal[1] = rotation[1][1];
normal[2] = rotation[2][1];
direction = MT_Vector3(0.0,-1.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHNZ:
normal[0] = rotation[0][2];
normal[1] = rotation[1][2];
normal[2] = rotation[2][2];
direction = MT_Vector3(0.0,0.0,-1.0);
break;
}
normal.normalize();
{
PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
PHY_IPhysicsController *spc = obj->GetPhysicsController();
if (!pe) {
std::cout << "WARNING: Constraint actuator " << GetName() << ": There is no physics environment!" << std::endl;
goto CHECK_TIME;
}
if (!spc || !spc->IsDynamic()) {
// the object is not dynamic, it won't support setting speed
goto CHECK_TIME;
}
m_hitObject = NULL;
// distance of Fh area is stored in m_minimum
MT_Point3 topoint = position + (m_minimumBound+spc->GetRadius()) * direction;
KX_RayCast::Callback<KX_ConstraintActuator> callback(this, spc);
result = KX_RayCast::RayTest(pe, position, topoint, callback);
// we expect a hit object
if (!m_hitObject)
result = false;
if (result)
{
MT_Vector3 newnormal = callback.m_hitNormal;
// compute new position & orientation
MT_Scalar distance = (callback.m_hitPoint-position).length()-spc->GetRadius();
// estimate the velocity of the hit point
MT_Point3 relativeHitPoint;
relativeHitPoint = (callback.m_hitPoint-m_hitObject->NodeGetWorldPosition());
MT_Vector3 velocityHitPoint = m_hitObject->GetVelocity(relativeHitPoint);
MT_Vector3 relativeVelocity = spc->GetLinearVelocity() - velocityHitPoint;
MT_Scalar relativeVelocityRay = direction.dot(relativeVelocity);
MT_Scalar springExtent = 1.0 - distance/m_minimumBound;
// Fh force is stored in m_maximum
MT_Scalar springForce = springExtent * m_maximumBound;
// damping is stored in m_refDirection [0] = damping, [1] = rot damping
MT_Scalar springDamp = relativeVelocityRay * m_refDirVector[0];
MT_Vector3 newVelocity = spc->GetLinearVelocity()-(springForce+springDamp)*direction;
if (m_option & KX_ACT_CONSTRAINT_NORMAL)
{
newVelocity+=(springForce+springDamp)*(newnormal-newnormal.dot(direction)*direction);
}
spc->SetLinearVelocity(newVelocity, false);
if (m_option & KX_ACT_CONSTRAINT_DOROTFH)
{
MT_Vector3 angSpring = (normal.cross(newnormal))*m_maximumBound;
MT_Vector3 angVelocity = spc->GetAngularVelocity();
// remove component that is parallel to normal
angVelocity -= angVelocity.dot(newnormal)*newnormal;
MT_Vector3 angDamp = angVelocity * ((m_refDirVector[1]>MT_EPSILON)?m_refDirVector[1]:m_refDirVector[0]);
spc->SetAngularVelocity(spc->GetAngularVelocity()+(angSpring-angDamp), false);
}
} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
// no contact but still keep running
result = true;
}
// don't set the position with this constraint
goto CHECK_TIME;
}
break;
case KX_ACT_CONSTRAINT_LOCX:
case KX_ACT_CONSTRAINT_LOCY:
case KX_ACT_CONSTRAINT_LOCZ:
newposition = position = obj->GetSGNode()->GetLocalPosition();
switch (m_locrot) {
case KX_ACT_CONSTRAINT_LOCX:
Clamp(newposition[0], m_minimumBound, m_maximumBound);
break;
case KX_ACT_CONSTRAINT_LOCY:
Clamp(newposition[1], m_minimumBound, m_maximumBound);
break;
case KX_ACT_CONSTRAINT_LOCZ:
Clamp(newposition[2], m_minimumBound, m_maximumBound);
break;
}
result = true;
if (m_posDampTime) {
newposition = filter*position + (1.0-filter)*newposition;
}
obj->NodeSetLocalPosition(newposition);
goto CHECK_TIME;
}
if (result) {
// set the new position but take into account parent if any
obj->NodeSetWorldPosition(newposition);
}
CHECK_TIME:
if (result && m_activeTime > 0 ) {
if (++m_currentTime >= m_activeTime)
result = false;
}
}
if (!result) {
m_currentTime = 0;
}
return result;
} /* end of KX_ConstraintActuator::Update(double curtime,double deltatime) */