Tried to solve through the rotation matrix, but it does not work
Input:
Ten reals:
The 3D coordinates of the vector.
The rotation angle in radians.
The 3D coordinates of a point of the axis.
The 3D coordinates of the direction vector of the axis.
The length of the direction vector is greater than 1e-8.
Output:
Three reals, the 3D coordinates of the vector after its rotation.
Example:
Input:
3.0 2.0 4.0 3.141592653 0.0 0.0 0.0 0.0 1.0 0.0
Output:
-3.0 2.0 -4.0
My realization of Vector3:
class Vector3D
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
public double Length
{
get
{
return Math.Sqrt(X * X + Y * Y + Z * Z);
}
}
public Vector3D(double x, double y, double z)
{
X = x;
Y = y;
Z = z;
}
public Vector3D()
{
X = 0;
Y = 0;
Z = 0;
}
public static Vector3D operator +(Vector3D w, Vector3D v) {
return new Vector3D(w.X + v.X, w.Y + v.Y, w.Z + v.Z);
}
public static Vector3D operator -(Vector3D w, Vector3D v)
{
return new Vector3D(w.X - v.X, w.Y - v.Y, w.Z - v.Z);
}
public static Vector3D operator *(double a, Vector3D v)
{
return new Vector3D(a * v.X, a * v.Y, a * v.Z);
}
public Vector3D RotateVector(Vector3D axis, double angle)
{
Vector3D vxp = axis.Cross(this);
Vector3D vxvxp = axis.Cross(vxp);
return this + Math.Sin(angle) * vxp + (1 - Math.Cos(angle)) * vxvxp;
}
public Vector3D RotateAboutPoint(Vector3D pivot, Vector3D axis, double angle)
{
return pivot + (this - pivot).RotateVector(axis, angle);
}
public void Normalize()
{
var div = 1 / Length;
X *= div;
Y *= div;
Z *= div;
}
public Vector3D Cross(Vector3D v)
{
return new Vector3D(Y * v.Z - Z * v.Y, - X * v.Z + Z * v.X, X * v.Y - Y * v.X);
}
}
Subtract the point of rotation from the vector
Rotate using a rotation matrix or whatever way you want.
Add the point of rotation to the result
In [C#] world the above is as follows:
/// <summary>
/// Rotates a vector using the Rodriguez rotation formula
/// about an arbitrary axis.
/// </summary>
/// <param name="vector">The vector to be rotated.</param>
/// <param name="axis">The rotation axis.</param>
/// <param name="angle">The rotation angle.</param>
/// <returns>The rotated vector</returns>
public static Vector3 RotateVector(Vector3 vector, Vector3 axis, float angle)
{
Vector3 vxp = Vector3.Cross(axis, vector);
Vector3 vxvxp = Vector3.Cross(axis, vxp);
return vector + Sin(angle) * vxp + (1 - Cos(angle)) * vxvxp;
}
/// <summary>
/// Rotates a vector about a point in space.
/// </summary>
/// <param name="vector">The vector to be rotated.</param>
/// <param name="pivot">The pivot point.</param>
/// <param name="axis">The rotation axis.</param>
/// <param name="angle">The rotation angle.</param>
/// <returns>The rotated vector</returns>
public static Vector3 RotateVectorAboutPoint(Vector3 vector, Vector3 pivot, Vector3 axis, float angle)
{
return pivot+ RotateVector(vector - pivot, axis, angle);
}
with the driver code
static void Main(string[] args)
{
Vector3 vector = new Vector3(3.0f, 2.0f, 4.0f);
float angle = 3.141592653f;
Vector3 pivot = new Vector3(0.0f, 0.0f, 0.0f);
Vector3 axis = new Vector3(0.0f, 1.0f, 0.0f);
Vector3 result = RotateAboutPoint(vector, pivot, axis, angle);
Console.WriteLine(result);
// <-3, 2, -4>
}
Related
using UnityEngine;
using System.Collections;
[RequireComponent(typeof(LineRenderer))]
public class DrawRadiusAround : MonoBehaviour
{
[Range(0, 50)]
public int segments = 50;
[Range(0, 5)]
public float xradius = 5;
[Range(0, 5)]
public float yradius = 5;
LineRenderer line;
void Start()
{
line = gameObject.GetComponent<LineRenderer>();
line.positionCount = segments + 1;
line.useWorldSpace = false;
CreatePoints();
}
void CreatePoints()
{
float x;
float y;
float z;
float angle = 20f;
for (int i = 0; i < (segments + 1); i++)
{
x = Mathf.Sin(Mathf.Deg2Rad * angle) * xradius;
y = Mathf.Cos(Mathf.Deg2Rad * angle) * yradius;
line.SetPosition(i, new Vector3(x, y, 0));
angle += (360f / segments);
}
}
}
Some problems :
The circle is standing vertical and not like it should be horizontal.
How can I make that I will be able to change in run time the radius size and the circle width ?
How can I make that in the editor mode before running the game it will not show the line renderer first
point near the target object that it should be around ?
The circle in run time is not completed at the top of it there is a place with some space or a missing
part.
This screen show is showing the start point or the line renderer in pink near the object before running the game :
This screenshot is showing the drawn circle of the radius and the linerenderer the script settings in run time :
To make the circle horizontal, vary the Z coordinate instead of the Y coordinate:
// ...
x = Mathf.Sin(Mathf.Deg2Rad * angle) * xradius;
y = Mathf.Cos(Mathf.Deg2Rad * angle) * yradius;
line.SetPosition(i, new Vector3(x, 0f, y));
// ..
To change the line width, vary the value of line.widthMultiplier:
line.widthMultiplier = 2f;
To change the radius, alter xradius and yradius then call CreatePoints again:
xradius = yradius = 10f;
CreatePoints();
The circle appears "incomplete" because the ends of the line renderer don't overlap sufficiently. To fix this, go further than 360 degrees by changing the 360f in the last line to something greater. For instance:
// ...
x = Mathf.Sin(Mathf.Deg2Rad * angle) * xradius;
y = Mathf.Cos(Mathf.Deg2Rad * angle) * yradius;
line.SetPosition(i, new Vector3(x, 0f, y));
angle += (380f / segments);
// ...
So unlike everyone here, I'm trying to find the intersections from circle-line collision.
The points are taken from the user input.
float cx; //circle center x
float cy; //circle center y
float px; //point x
float py; //point y
float vx; //vector x
float vy; //vector y
float vySq = vy * vy;
float vxSq = vx * vx;
float cxSq = cx * cx;
float rSq = r * r;
float thatPart = ( ( (vy * px) / vx) + py - cy); //so I don't have to re-type it 3 different times
float a = 1 + (vySq/vxSq);
float b = (2 * cx) + ( (2 * vy) * thatPart / vx);
float c = cxSq + ( thatPart * thatPart ) - rSq;
float x1 = QUADRATIC(a, b, c, true);
float x2 = QUADRATIC(a, b, c, false);
float y1 = ((vy * x1) - (vy * px) + (vx * py)) / vx;
float y2 = ((vy * x2) - (vy * px) + (vx * py)) / vx;
My QUADRATIC function 100% works, so I am not worried about that.
It's a, b, and c. I receive the wrong values for them. And all this math is based on a pdf given in math class. (here)
So... what am I doing wrong?
Thanks.
The equations in both the PDF and the source code seem unnecessarily complicated, so I will re-establish them in a clenear way (or at least one that pleases me).
The vector from the center of the circle to any point S on the line can be written as
CS = CP + t V
where t is an arbitrary parameter.
S belongs to the circle when
CS² = R² = (CP + t V)²
giving the quadratic equation in t:
V² t² + 2 CP.V t + CP² - R² = 0
When you have the values of t, plug in the first equation.
Math Treatment
You have a line with coordinates (x,y) = (px + vx*t, py + vy*t) where t is an arbitrary parameter.
The line intersects the circle when its coordinates solve x^2 + y^2 = r^2.
This leads to the following quadratic equation, to be solved for t.
t^2 + 2*t*b + a = 0
a = (px^2+py^2-r^2)/(vx^2+vy^2)
b = (px*vx+py*vy)/(vx^2+vy^2)
The two solutions are:
t = -b ± sqrt(b^2-a)
Use the two t values into (x,y) to get the coordinates.
Sample Code
static class Program
{
static void Main(string[] args)
{
var ray = new Ray() {
Point = new Vector2(-3, 1),
Direction = new Vector2(2, 0.5f)
};
var circle = new Circle
{
Center = new Vector2(1, 1),
Radius = 4
};
var points = Geometry.Intersect(circle, ray);
if(points.Length>0)
{
foreach(var point in points)
{
Console.WriteLine(point.ToString());
}
}
else
{
Console.WriteLine("Circle and Ray do not intersect");
}
}
}
Object Oriented Models
public class Circle
{
public Vector2 Center { get; set; }
public float Radius { get; set; }
}
public class Ray
{
public Vector2 Point { get; set; }
public Vector2 Direction { get; set; }
public Vector2 Along(float t)
{
return Point + t*Direction;
}
}
public static class Geometry
{
public static Vector2[] Intersect(Circle circle, Ray ray)
{
float a = (ray.Point.LengthSquared()-circle.Radius*circle.Radius)/ray.Direction.LengthSquared();
float b = Vector2.Dot(ray.Point, ray.Direction)/ray.Direction.LengthSquared();
if(b*b-a>0)
{
// two intersection points
float t1 = -b-(float)Math.Sqrt(b*b-a);
float t2 = -b+(float)Math.Sqrt(b*b-a);
return new Vector2[] {
ray.Along(t1),
ray.Along(t2),
};
}
else if(b*b-a==0)
{
// one intersection point
float t = -b;
return new Vector2[] { ray.Along(t) };
}
else
{
// no intersection, return empty array
return new Vector2[0];
}
}
}
NOTE: Code uses System.Numerics library
I have the following C++ code that i want to translate to C#
But i'm stuck in the definition of "Element" which is a structured array.
What is best to use ?
var list = new List<KeyValuePair<string, int>>();
for (i = 0; i< 8; i++)
{
Ixx += Element[i].vLocalInertia.x + Element[i].fMass * (Element[i].vCGCoords.y*Element[i].vCGCoords.y + Element[i].vCGCoords.z*Element[i].vCGCoords.z);
Iyy += Element[i].vLocalInertia.y + Element[i].fMass * (Element[i].vCGCoords.z*Element[i].vCGCoords.z + Element[i].vCGCoords.x*Element[i].vCGCoords.x);
Izz += Element[i].vLocalInertia.z + Element[i].fMass * (Element[i].vCGCoords.x*Element[i].vCGCoords.x + Element[i].vCGCoords.y*Element[i].vCGCoords.y);
Ixy += Element[i].fMass * (Element[i].vCGCoords.x * Element[i].vCGCoords.y);
Ixz += Element[i].fMass * (Element[i].vCGCoords.x * Element[i].vCGCoords.z);
Iyz += Element[i].fMass * (Element[i].vCGCoords.y * Element[i].vCGCoords.z);
}
And have the following type struct :
typedef struct _RigidBody {
float fMass; // total mass (constant)
Matrix3x3 mInertia; // mass moment of inertia in body coordinates (constant)
Matrix3x3 mInertiaInverse;// inverse of mass moment of inertia matrix (constant)
Vector vPosition; // position in earth coordinates
Vector vVelocity; // velocity in earth coordinates
Vector vVelocityBody; // velocity in body coordinates
Vector vAngularVelocity;// angular velocity in body coordinates
Vector vEulerAngles; // Euler angles in body coordinates
float fSpeed; // speed (magnitude of the velocity)
Quaternion qOrientation; // orientation in earth coordinates
//Matrix3x3 mRotation; // rotation matrix
Vector vForces; // total force on body
Vector vMoments; // total moment (torque) on body
Matrix3x3 mIeInverse; // inverse of moment of inertia in earth coordinates
} RigidBody, *pRigidBody;
typedef struct _BodyElement {
float fMass;
Vector vDCoords;
Vector vCGCoords;
Vector vLocalInertia;
float fIncidence;
float fDihedral;
Vector vNormal;
float fArea;
int iFlap;
} BodyElement, *pBodyElement;
Something like this?
public struct Element
{
public struct coord
{
public int x;
public int y;
public int z;
}
public coord vLocalIntetia;
public int fMass;
public coord vCGCoords;
}
Element[] element = new Element[8];
EDIT
Or this (with your added detail)
public struct BodyElement
{
float fMass;
Vector vDCoords;
Vector vCGCoords;
Vector vLocalInertia;
float fIncidence;
float fDihedral;
Vector vNormal;
float fArea;
int iFlap;
}
BodyElement[] element = new BodyElement[8];
So basicly I want to reflect a ray over a triangle. Here's my ray class
public sealed class Ray
{
public readonly Point3D Source;
public readonly Point3D Direction;
public readonly Color Light;
public Ray(Point3D source, Point3D direction, Color light)
{
if (source == direction)
{
throw new ArgumentException("Source and Direction cannot be equal");
}
this.Source = source;
this.Direction = direction;
this.Light = light;
}
}
Heres my Point3D class
public struct Point3D : IEquatable<Point3D>
{
public static readonly Point3D Zero = new Point3D();
public float X;
public float Y;
public float Z;
public Point3D(float x, float y, float z)
{
this.X = x;
this.Y = y;
this.Z = z;
}
public override bool Equals(object obj)
{
if (!(obj is Point3D))
{
return false;
}
return this.Equals((Point3D)obj);
}
public static bool operator ==(Point3D one, Point3D two)
{
return one.Equals(two);
}
public static bool operator !=(Point3D one, Point3D two)
{
return !one.Equals(two);
}
public static Point3D operator *(float n, Point3D v)
{
return new Point3D(v.X * n, v.Y * n, v.Z * n);
}
public static Point3D operator +(Point3D v1, Point3D v2)
{
return new Point3D(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
}
public static Point3D operator -(Point3D v1, Point3D v2)
{
return new Point3D(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
}
public static float operator *(Point3D v1, Point3D v2)
{
return (v1.X * v2.X) + (v1.Y * v2.Y) + (v1.Z * v2.Z);
}
public static float Magnitude(Point3D v)
{
return (float)Math.Sqrt(v * v);
}
public static Point3D Normalize(Point3D v)
{
float mag = Magnitude(v);
float div = (mag == 0) ? float.PositiveInfinity : (1 / mag);
return div * v;
}
public static Point3D Cross(Point3D v1, Point3D v2)
{
return new Point3D(((v1.Y * v2.Z) - (v1.Z * v2.Y)),
((v1.Z * v2.X) - (v1.X * v2.Z)),
((v1.X * v2.Y) - (v1.Y * v2.X)));
}
/// <summary>
/// doesnt take square root
/// </summary>
public static float FastDistance(Point3D v1, Point3D v2)
{
float x = v1.X - v2.X;
x *= x;
float y = v1.Y - v2.Y;
y *= y;
float z = v1.Z - v2.Z;
z *= z;
return x + y + z;
}
/// <summary>
/// Takes square root:
/// </summary>
public static float Distance(Point3D v1, Point3D v2)
{
return (float)Math.Sqrt(Point3D.FastDistance(v1, v2));
}
public override int GetHashCode()
{
return this.X.GetHashCode()
^ this.Y.GetHashCode()
^ this.Y.GetHashCode();
}
public override string ToString()
{
return this.X + ", " + this.Y + ", " + this.Z;
}
public bool Equals(Point3D other)
{
return this.X == other.X
&& this.Y == other.Y
&& this.Z == other.Z;
}
}
and finally here is where I need my method to be realized.
public interface ITriangleAccess
{
Triangle3D Find(Ray ray, out Point3D crossPoint);
}
public sealed class TriangleAccess : ITriangleAccess
{
private readonly List<KeyValuePair<float, Triangle3D>> trianglesByX;
private readonly List<Triangle3D> allTriangles;
public TriangleAccess(Body[] bodies)
{
if (null == bodies)
{
throw new ArgumentNullException("bodies");
}
this.allTriangles = bodies.SelectMany((x) => x.Parts).ToList();
this.trianglesByX = bodies.SelectMany((x) => x.Parts).SelectMany((y) => new KeyValuePair<float, Triangle3D>[]
{
new KeyValuePair<float,Triangle3D>(y.Point1.X,y),
new KeyValuePair<float,Triangle3D>(y.Point2.X,y),
new KeyValuePair<float,Triangle3D>(y.Point3.X,y)
}).ToList();
}
public Triangle3D Find(Ray ray, out Point3D crossPoint)
{
crossPoint = Point3D.Zero;
List<Triangle3D> relevant = this.GetRelevantTriangles(ray);
Triangle3D absoluteTriangle = null;
float min = float.MaxValue;
foreach (Triangle3D item in relevant)
{
Point3D currentCrossPoint;
if (this.RayIntersectTriangle(ray, item, out currentCrossPoint))
{
float distance = Point3D.Distance(ray.Source, currentCrossPoint);
if (distance < min)
{
absoluteTriangle = item;
crossPoint = currentCrossPoint;
min = distance;
}
}
}
return absoluteTriangle;
}
public Ray Reflect(Ray ray, Point3D crossPoint, Triangle3D intersect)
{
//need this to be realized
//please help
}
/// <summary>
/// TODO: Finish this Up:
/// </summary>
/// <param name="ray"></param>
/// <returns></returns>
private List<Triangle3D> GetRelevantTriangles(Ray ray)
{
return this.allTriangles;
}
private bool RayIntersectTriangle(Ray ray, Triangle3D triangle, out Point3D crossPoint)
{
// Find vectors for two edges sharing vert0
Point3D edge1 = triangle.Point2 - triangle.Point1;
Point3D edge2 = triangle.Point3 - triangle.Point1;
// Begin calculating determinant - also used to calculate barycentricU parameter
Point3D pvec = Point3D.Cross(ray.Direction, edge2);
// If determinant is near zero, ray lies in plane of triangle
float det = edge1 * pvec;
if (det < 0.0001f)
{
crossPoint = Point3D.Zero;
return false;
}
// Calculate distance from vert0 to ray origin
Point3D tvec = ray.Source - triangle.Point1;
// Calculate barycentricU parameter and test bounds
float barycentricU = tvec * pvec;
if (barycentricU < 0.0f || barycentricU > det)
{
crossPoint = Point3D.Zero;
return false;
}
// Prepare to test barycentricV parameter
Point3D qvec = Point3D.Cross(tvec, edge1);
// Calculate barycentricV parameter and test bounds
float barycentricV = ray.Direction * qvec;
if (barycentricV < 0.0f || barycentricU + barycentricV > det)
{
crossPoint = Point3D.Zero;
return false;
}
// Calculate pickDistance, scale parameters, ray intersects triangle
float pickDistance = edge2 * qvec;
float fInvDet = 1.0f / det;
pickDistance *= fInvDet;
barycentricU *= fInvDet;
barycentricV *= fInvDet;
crossPoint = MathHelper.BaryCentric(triangle, barycentricU, barycentricV);
return true;
}
}
Thank you.
P.S have patience with me... I'm just 15 :)
I'm assuming that if you have a ray that intersects with a triangle, your Reflect function should return a ray that originates at that intersection and heads off in a new direction (like light on a mirror).
That said, we already have the ray source (crossPoint).
The formula for the reflected vector (direction of the ray) is R=V−2N(V⋅N) where V is the inverted direction of the incoming ray (as if it originated from the object) and N is the normal vector of the triangle.
With your code, you should just need to something like:
public Ray Reflect(Ray ray, Point3D crossPoint, Triangle3D intersect)
{
Point3D V = -ray.Direction.Normalize();
return new Ray(crossPoint,
V - 2 * intersect.Normal * (V * intersect.Normal),
Color.white);
}
Also, if your triangle structure doesn't have the normal, you calculate it using the cross product of the vectors that form each side.
public Ray Reflect(Ray ray, Point3D crossPoint, Triangle3D intersect)
{
// find normal of intersect triangle
Point3D normal = Point3D.Cross( intersect.Point2 - intersect.Point1,
intersect.Point3 - intersect.Point1 );
normal = Point3D.Normalize( normal );
// find ray part before intersection
Point3D inbound = crossPoint - ray.Source;
// find projection of inbound ray to normal
Point3D projection = (normal * inbound) * normal;
// find lateral component of inbound ray
Point3D lateral = inbound - projection;
// find outbound direction
Point3D direction = (ray.Source + 2 * lateral) - crossPoint;
direction = Point3D.Normalize( direction );
// I assume your direction is unit vector (magnitude = 1)
// if not multiply it with magnitude you want to be
// direction = ... * direction;
return new Ray( crossPoint, direction, ray.Color );
}
This question already has an answer here:
Finding Points On Perimeter Of a Circle
(1 answer)
Closed 10 years ago.
Given:
x = originX + radius * Cos(angle);
y = originY + radius * Sin(angle);
Why would the points not be evenly distributed around the edge of the circle?
Image of the result:
class Circle
{
public Vector2 Origin { get; set; }
public float Radius { get; set; }
public List<Vector2> Points { get; set; }
private Texture2D _texture;
public Circle(float radius, Vector2 origin, ContentManager content)
{
Radius = radius;
Origin = origin;
_texture = content.Load<Texture2D>("pixel");
Points = new List<Vector2>();
//i = angle
for (int i = 0; i < 360; i++)
{
float x = origin.X + radius * (float)Math.Cos(i);
float y = origin.Y + radius * (float)Math.Sin(i);
Points.Add(new Vector2(x, y));
}
}
public void Draw(SpriteBatch spriteBatch)
{
for (int i = 0; i < Points.Count; i++)
spriteBatch.Draw(_texture, Points[i], new Rectangle(0, 0, _texture.Width, _texture.Height), Color.Red);
}
}
Math.Cos and Math.Sin take the angle in radians as opposed to degrees, your code should be:
float x = origin.X + radius * (float)Math.Cos(i*Math.PI/180.0);
float y = origin.Y + radius * (float)Math.Sin(i*Math.PI/180.0);
Points:
1) Math.Trig functions use Radians, not Degrees.
2) For this kind of precision, you would be better off using double instead of float.
3) The computer graphics/gaming pros avoid expensive functions like Sin and Cos, and instead use incremental integer pixel oriented approaches, like Bresenham's Algorithms, that give results as good or better than the straight-forward trigonometric math calculations and are much faster.