public void changeOrientation(Quaternion newrot)
{
if (m_isphysical)
{
if (childPrim)
{
if (m_blockPhysicalReconstruction) // inertia is messed, must rebuild
{
_orientation = newrot;
}
}
else
{
if (newrot != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = newrot;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newrot;
if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
}
if (Body != IntPtr.Zero)
d.BodyEnable(Body);
}
else
{
if (newrot != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = newrot;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newrot;
}
}
if (--fakeori < 0)
fakeori = 0;
changeSelectedStatus(m_isSelected);
resetCollisionAccounting();
}
public void changeprimsizeshape()
{
_parent_scene.actor_name_map.Remove(prim_geom);
UniverseODEPrim parent = null;
bool chp = childPrim;
if (chp)
parent = (UniverseODEPrim) _parent;
// Cleanup of old prim geometry and Bodies
if (chp)
{
if (parent != null)
parent.DestroyBody();
}
else
{
DestroyBody();
}
if (prim_geom != IntPtr.Zero)
{
try
{
d.GeomDestroy(prim_geom);
}
catch (AccessViolationException)
{
prim_geom = IntPtr.Zero;
MainConsole.Instance.Error("[PHYSICS]: PrimGeom dead");
}
prim_geom = IntPtr.Zero;
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0)
_size.X = 0.01f;
if (_size.Y <= 0)
_size.Y = 0.01f;
if (_size.Z <= 0)
_size.Z = 0.01f;
CreateGeom(m_targetSpace);
CalcPrimBodyData();
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_parent_scene.actor_name_map[prim_geom] = this;
}
changeSelectedStatus(m_isSelected);
if (chp)
{
if (parent != null)
{
parent.MakeBody();
}
}
else
MakeBody();
}
public void changeadd()
{
// all prims are now created non physical
IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
m_targetSpace = targetspace;
//Console.WriteLine("changeadd 1");
CreateGeom(m_targetSpace);
CalcPrimBodyData();
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
// _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
}
changeSelectedStatus(m_isSelected);
}
private void UpdateDataFromGeom()
{
if (prim_geom != IntPtr.Zero)
{
d.Vector3 lpos = d.GeomGetPosition(prim_geom);
_position.X = lpos.X;
_position.Y = lpos.Y;
_position.Z = lpos.Z;
d.Quaternion qtmp = new d.Quaternion
{
};
d.GeomCopyQuaternion(prim_geom, out qtmp);
_orientation.W = qtmp.W;
_orientation.X = qtmp.X;
_orientation.Y = qtmp.Y;
_orientation.Z = qtmp.Z;
}
}
private void MakeBody()
{
// d.Vector3 dvtmp;
// d.Vector3 dbtmp;
if (m_blockPhysicalReconstruction) // building is blocked
return;
if (childPrim) // child prims don't get own bodies;
return;
if (prim_geom == IntPtr.Zero)
{
MainConsole.Instance.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
return;
}
if (!m_isphysical) // only physical things get a body
return;
if (Body != IntPtr.Zero) // who shouldn't have one already ?
{
d.BodyDestroy(Body);
Body = IntPtr.Zero;
MainConsole.Instance.Warn("[PHYSICS]: MakeBody called having a body");
}
d.Mass objdmass = new d.Mass {};
d.Matrix3 mymat = new d.Matrix3();
d.Quaternion myrot = new d.Quaternion();
Body = d.BodyCreate(_parent_scene.world);
d.BodySetData(Body, (IntPtr)ActorTypes.Prim);
DMassDup(ref primdMass, out objdmass);
// rotate inertia
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.RfromQ(out mymat, ref myrot);
d.MassRotate(ref objdmass, ref mymat);
// set the body rotation and position
d.BodySetRotation(Body, ref mymat);
// recompute full object inertia if needed
if (childrenPrim.Count > 0)
{
d.Matrix3 mat = new d.Matrix3();
d.Quaternion quat = new d.Quaternion();
d.Mass tmpdmass = new d.Mass {};
Vector3 rcm;
rcm.X = _position.X + objdmass.c.X;
rcm.Y = _position.Y + objdmass.c.Y;
rcm.Z = _position.Z + objdmass.c.Z;
lock (childrenPrim)
{
foreach (UniverseODEPrim prm in childrenPrim)
{
if (prm.prim_geom == IntPtr.Zero)
{
MainConsole.Instance.Warn(
"[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
continue;
}
DMassCopy(ref prm.primdMass, ref tmpdmass);
// apply prim current rotation to inertia
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.RfromQ(out mat, ref quat);
d.MassRotate(ref tmpdmass, ref mat);
Vector3 ppos = prm._position;
ppos.X += tmpdmass.c.X - rcm.X;
ppos.Y += tmpdmass.c.Y - rcm.Y;
ppos.Z += tmpdmass.c.Z - rcm.Z;
// refer inertia to root prim center of mass position
d.MassTranslate(ref tmpdmass,
ppos.X,
ppos.Y,
ppos.Z);
d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
// fix prim colision cats
d.GeomClearOffset(prm.prim_geom);
d.GeomSetBody(prm.prim_geom, Body);
prm.Body = Body;
d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
}
}
}
d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
// associate root geom with body
d.GeomSetBody(prim_geom, Body);
d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z);
// ode wants inertia at center of body
myrot.W = -myrot.W;
d.RfromQ(out mymat, ref myrot);
d.MassRotate(ref objdmass, ref mymat);
d.BodySetMass(Body, ref objdmass);
_mass = objdmass.mass;
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
// disconnect from world gravity so we can apply buoyancy
// if (!testRealGravity)
d.BodySetGravityMode(Body, false);
d.BodySetAutoDisableFlag(Body, true);
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
d.BodySetDamping(Body, .001f, .0002f);
m_disabled = false;
d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
if (m_targetSpace != _parent_scene.space)
{
if (d.SpaceQuery(m_targetSpace, prim_geom))
d.SpaceRemove(m_targetSpace, prim_geom);
m_targetSpace = _parent_scene.space;
d.SpaceAdd(m_targetSpace, prim_geom);
}
lock (childrenPrim)
{
foreach (UniverseODEPrim prm in childrenPrim)
{
if (prm.prim_geom == IntPtr.Zero)
continue;
Vector3 ppos = prm._position;
d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
d.GeomSetCategoryBits(prm.prim_geom, (int) prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int) prm.m_collisionFlags);
if (prm.m_targetSpace != _parent_scene.space)
{
if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
prm.m_targetSpace = _parent_scene.space;
d.SpaceAdd(m_targetSpace, prm.prim_geom);
}
prm.m_disabled = false;
_parent_scene.addActivePrim(prm);
}
}
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
{
createAMotor(m_angularlock);
}
if (m_vehicle.Type != Vehicle.TYPE_NONE)
m_vehicle.Enable(Body, this, _parent_scene);
_parent_scene.addActivePrim(this);
}
private d.Quaternion ConvertTodQuat(Quaternion q)
{
d.Quaternion dq = new d.Quaternion {X = q.X, Y = q.Y, Z = q.Z, W = q.W};
return dq;
}
public void SetRotationLocked(Quaternion taintRot)
{
d.Quaternion q = new d.Quaternion
{
W = taintRot.W,
X = taintRot.X,
Y = taintRot.Y,
Z = taintRot.Z
};
d.BodySetQuaternion(Body, ref q); // just keep in sync with rest of simutator
}