public override void DrawTriangle(ref Vector3 v0, ref Vector3 v1, ref Vector3 v2, ref Vector3 color, float alpha)
{
UnityEngine.Gizmos.color = new UnityEngine.Color(color.X, color.Y, color.Z);
UnityEngine.Gizmos.DrawLine(v0.ToUnity(), v1.ToUnity());
UnityEngine.Gizmos.DrawLine(v1.ToUnity(), v2.ToUnity());
UnityEngine.Gizmos.DrawLine(v2.ToUnity(), v0.ToUnity());
}
public bool Raycast(Vector3 origin, Vector3 direction, out Uniject.RaycastHit hitinfo, float distance, int layerMask)
{
UnityEngine.RaycastHit unityHit = new UnityEngine.RaycastHit();
bool result = UnityEngine.Physics.Raycast(origin.ToUnity(), direction.ToUnity(), out unityHit, distance, layerMask);
if (result)
{
IGameObject testable = null;
var bridge = unityHit.collider.gameObject.GetComponent <UnityBridgeComponent>();
if (null != bridge)
{
testable = bridge.GameObject;
}
hitinfo = new RaycastHit(unityHit.point.ToUniject(),
unityHit.normal.ToUniject(),
unityHit.barycentricCoordinate.ToUniject(),
unityHit.distance,
unityHit.triangleIndex,
unityHit.textureCoord.ToUniject(),
unityHit.textureCoord2.ToUniject(),
unityHit.lightmapCoord.ToUniject(),
testable,
unityHit.collider.ToUniject());
}
else
{
hitinfo = new RaycastHit();
}
return(result);
}
public bool Raycast(Vector3 origin, Vector3 direction, out Uniject.RaycastHit hitinfo, float distance, int layerMask)
{
UnityEngine.RaycastHit unityHit = new UnityEngine.RaycastHit();
bool result = UnityEngine.Physics.Raycast(origin.ToUnity(), direction.ToUnity(), out unityHit, distance, layerMask);
if (result) {
IGameObject testable = null;
var bridge = unityHit.collider.gameObject.GetComponent<UnityBridgeComponent>();
if (null != bridge) {
testable = bridge.GameObject;
}
hitinfo = new RaycastHit (unityHit.point.ToUniject(),
unityHit.normal.ToUniject(),
unityHit.barycentricCoordinate.ToUniject(),
unityHit.distance,
unityHit.triangleIndex,
unityHit.textureCoord.ToUniject(),
unityHit.textureCoord2.ToUniject(),
unityHit.lightmapCoord.ToUniject(),
testable,
unityHit.collider.ToUniject());
} else {
hitinfo = new RaycastHit();
}
return result;
}
public override void DrawBox(ref Vector3 bbMin, ref Vector3 bbMax, ref Vector3 color)
{
UnityEngine.Bounds b = new UnityEngine.Bounds(bbMin.ToUnity(), UnityEngine.Vector3.zero);
b.Encapsulate(bbMax.ToUnity());
UnityEngine.Gizmos.color = new UnityEngine.Color(color.X, color.Y, color.Z);
UnityEngine.Gizmos.DrawWireCube(b.center, b.size);
}
public override void DrawPlane(ref Vector3 planeNormal, float planeConst, ref Matrix trans, ref Vector3 color)
{
UnityEngine.Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
UnityEngine.Vector3 scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawPlane(pos, rot, scale, planeNormal.ToUnity(), planeConst, c);
}
public void ApplyMotorTorque(Vector3 v, bool convertFromRos = false)
{
useTwist = false;
torque = v;
if (convertFromRos)
{
torque = torque.ToUnity();
}
// torque *= convertFromRos ? -torqueForce : torqueForce;
}
public void ApplyMotorForce(Vector3 v, bool convertFromRos = false)
{
useTwist = false;
force = v;
if (convertFromRos)
{
force = force.ToUnity();
}
// force *= thrustForce;
}
public void SetPositionAndOrientation(Vector3 pos, Quaternion orientation, bool convertFromRos = false)
{
setPoseFlag = true;
if (convertFromRos)
{
posePosition = pos.ToUnity();
poseOrientation = orientation.ToUnity();
}
else
{
posePosition = pos;
poseOrientation = orientation;
}
}
public override void DrawLine(ref Vector3 from, ref Vector3 to, ref Vector3 fromColor)
{
UnityEngine.Gizmos.color = new UnityEngine.Color(fromColor.X, fromColor.Y, fromColor.Z);
UnityEngine.Gizmos.DrawLine(from.ToUnity(), to.ToUnity());
}
public override void DrawArc(ref Vector3 center, ref Vector3 normal, ref Vector3 axis, float radiusA, float radiusB, float minAngle, float maxAngle,
ref Vector3 color, bool drawSect, float stepDegrees)
{
UnityEngine.Color col = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawArc(center.ToUnity(), normal.ToUnity(), axis.ToUnity(), radiusA, radiusB, minAngle, maxAngle, col, drawSect, stepDegrees);
}
public void LookAt(Vector3 point)
{
Transform.LookAt(point.ToUnity());
}
public void AddForce(Vector3 force)
{
this.body.AddForce(force.ToUnity());
}
public void AddTorque(Vector3 torque, ForceMode mode)
{
this.body.AddTorque(torque.ToUnity(), (UnityEngine.ForceMode)mode);
}
public bool Raycast(Vector3 origin, Vector3 direction, float distance, int layerMask)
{
return UnityEngine.Physics.Raycast(origin.ToUnity(), direction.ToUnity(), distance, layerMask);
}
public void Rotate(Vector3 eulerAngle, Space space = Space.Self)
{
Transform.Rotate(eulerAngle.ToUnity(), (UnityEngine.Space)space);
}
public void setDestination(Vector3 target)
{
agent.SetDestination(target.ToUnity());
}
public void Rotate(Vector3 eulerAngle, Space space = Space.Self)
{
Transform.Rotate(eulerAngle.ToUnity(), (UnityEngine.Space)space);
}
public override void DrawSphere(ref Vector3 p, float radius, ref Vector3 color)
{
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawSphere(p.ToUnity(), UnityEngine.Quaternion.identity, UnityEngine.Vector3.one, UnityEngine.Vector3.one * radius, c);
}
public Vector3 TransformDirection(Vector3 dir)
{
return(Transform.TransformDirection(dir.ToUnity()).ToUniject());
}
public void SetAngularVelocity(Vector3 v, bool convertFromRos = false)
{
useTwist = true;
force = torque = Vector3.zero;
AngularVelocity = convertFromRos ? v.ToUnity() : v;
}
public Vector3 TransformDirection(Vector3 dir)
{
return Transform.TransformDirection(dir.ToUnity()).ToUniject();
}
public void Translate(Vector3 byVector)
{
Transform.Translate(byVector.ToUnity());
}
public Quaternion LookRotation(Vector3 direction, Vector3 up)
{
return UnityEngine.Quaternion.LookRotation(direction.ToUnity(), up.ToUnity()).ToUniject();
}
void LateUpdate()
{
Vector3 rosForward = transform.forward.ToRos();
Vector3 rosRight = transform.right.ToRos();
Vector3 rosUp = transform.up.ToRos();
Quaternion q = transform.rotation.ToRos();
Debug.DrawRay(transform.position, rosForward * 5, Color.blue);
Debug.DrawRay(transform.position, rosRight * 5, Color.red);
Debug.DrawRay(transform.position, rosUp * 5, Color.green);
Vector3 up = Vector3.up * 0.2f;
Debug.DrawRay(transform.position + up, rosForward.ToUnity() * 5, Color.cyan);
Debug.DrawRay(transform.position + up, rosRight.ToUnity() * 5, Color.magenta);
Debug.DrawRay(transform.position + up, rosUp.ToUnity() * 5, Color.yellow);
// Debug.DrawRay ( transform.position, q * Vector3.up * 5, Color.magenta );
transform.rotation = Quaternion.identity;
transform.Rotate(rosRotation.ToUnity());
return;
Debug.DrawRay(transform.position, Vector3.forward * 5, Color.blue);
Debug.DrawRay(transform.position, Vector3.right * 5, Color.red);
Debug.DrawRay(transform.position, Vector3.up * 5, Color.green);
Matrix4x4 mat = Matrix4x4.identity;
mat.SetTRS(rosDirection, Quaternion.identity, Vector3.one);
// Vector3 unityDirection = Matrix4x4.identity.ToUnity ().MultiplyPoint3x4 ( rosDirection );
Vector3 unityDirection = mat.ToUnity().ExtractPosition();
Debug.DrawRay(transform.position, unityDirection * 5, Color.white);
mat.SetTRS(Vector3.forward, Quaternion.identity, Vector3.one);
// unityDirection = Matrix4x4.identity.ToUnity ().MultiplyPoint3x4 ( Vector3.forward );
unityDirection = mat.ToUnity().ExtractPosition();
Debug.DrawRay(transform.position, unityDirection * 5, Color.magenta);
// Vector3 v = ( toUnity * new Vector3 ( 0, 5, 0 ) * toRos );
// Vector3 unityDirection = toUnity * rosDirection.normalized;
// Vector3 rosDir = toRos * rosDirection.normalized;
// Debug.DrawRay ( transform.position, unityDirection * 5, Color.magenta );
// Debug.DrawRay ( transform.position, rosDir * 5, Color.cyan );
// transform.rotation =
// toRos = transform.localToWorldMatrix.ToUnity ();
// toUnity = toRos.inverse;
// forward = toUnity * Vector3.forward;
// right = toUnity * Vector3.right;
// up = toUnity * Vector3.up;
// forward = toRos * Vector3.forward;
// right = toRos * Vector3.right;
// up = toRos * Vector3.up;
// forward = toRos * transform.forward;
// right = toRos * transform.right;
// up = toRos * transform.up;
// Debug.DrawRay ( transform.position, forward * 5, Color.magenta );
// Debug.DrawRay ( transform.position, right * 5, Color.cyan );
// Debug.DrawRay ( transform.position, up * 5, Color.yellow );
}
public void AddTorque(Vector3 torque, ForceMode mode)
{
this.body.AddTorque(torque.ToUnity(), (UnityEngine.ForceMode)mode);
}
public void LookAt(Vector3 point)
{
Transform.LookAt(point.ToUnity());
}
public void AddForce(Vector3 force)
{
this.body.AddForce(force.ToUnity());
}
public Quaternion LookRotation(Vector3 direction, Vector3 up)
{
return(UnityEngine.Quaternion.LookRotation(direction.ToUnity(), up.ToUnity()).ToUniject());
}
public void setDestination(Vector3 target)
{
agent.SetDestination(target.ToUnity());
}
public bool Raycast(Vector3 origin, Vector3 direction, float distance, int layerMask)
{
return(UnityEngine.Physics.Raycast(origin.ToUnity(), direction.ToUnity(), distance, layerMask));
}
public void SetEyeTarget(Vector3 eye, Vector3 targ)
{
UnityEngine.Transform t = UnityEngine.Camera.main.transform;
t.position = eye.ToUnity();
t.rotation = UnityEngine.Quaternion.LookRotation((targ - eye).ToUnity().normalized, UnityEngine.Vector3.up);
}
public void Translate(Vector3 byVector)
{
Transform.Translate(byVector.ToUnity());
}
